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Given  a  desired  path  to  be  followed  by  a  Robot,  a  set  of  commands 
must  be  given  to  the  Robot  joint  servos  so  that  the  Robot  Tip,  or  Endpoint, 
can  follow  that  path.  These  commands  must  be  synchronized  in  time  and 
scaled  so  as  to  maintain  accuracy  in  the  presence  of  possible  saturations 
in  the  servos.  This  Thesis  develops  an  algorithm  to  generate  multiple 
simultaneous  time  varying  commands  to  Robot  joint  positioning  servos  so 
that  the  Robot  Tip  will  follow  a  desired  path  for  the  Cartesian  and 
Articulated  Robots. 


i  A ( 
v 

i  Ml.  ,  'r .  i  l 


I.  INTRODUCTION 


A.  TEACHING  METHODS . 

B.  PROGRAMMING  . 

C.  ROBOT  MODEL  . 

D.  PATH  APPLICATION  . 

E  MOTOR  CONTROL  . 

F.  OBJECTIVES  . 

I I .  MODELING  THE  ROBOT  MANIPULATOR 

A.  INTRODUCTION  . 

B.  CARTESIAN  ROBOT  MODEL  . 

C.  DERIVATION  OF  THE  LAGRANGIAN  EQUATION 

OF  MOTION  . 

III.  COMPUTER  SIMULATION  MODEL 

A.  INTRODUCTION  . 

B.  COMPUTER  MODEL  . 

C.  SERVOMOTOR  . 

D.  SERVO  MOTOR  SIMULATION  MODEL  . 

IV.  DSL  SIMULATION  PROGRAM 

A.  INTRODUCTION  . 

B.  PHILOSOPHY  OF  OPERATION  . 

C.  EXCITATION  PROPOSALS  . 

D.  MULTIPLE  RUNS  . 


h'  »■» 1,'  U-WI-LCUI.I'L 


IkWALU 


V 

V 

$ 


9 

•A 


V.  COMPUTER  PROGRAM  PATH  DEVELOPMENT 

A.  INTRODUCTION  .  73 

B.  MANUAL  TEACHING  AND  LEAD-THROUGH  TEACHING  .  73 

C.  PROGRAMMING  TECHNIQUES  .  74 

D.  COMPUTER  PATH  DEVELOPMENT  .  74 

1.  Desired  Path  .  75 

2.  Cartesian  Robot  Motion  .  77 

VI.  PATH  FOLLOWING  ROBOT  SIMULATIONS 

A.  INTRODUCTION  .  97 

B.  SCALING  .  98 

C.  TEST  RUNS  .  99 

D.  RESULTS 

1.  Linear  Motion  .  100 

2.  Circular  Motion  .  102 

3.  Helical  Motion  .  106 

4.  Sinusoidal  Motion  .  1 07 

E  POSITION  ACCURACY  .  108 

F.  ERROR  .  109 

VII .  ARTICULATED  ROBOT  MODEL 

A.  INTRODUCTION  .  172 

B.  COORDINATE  TRANSFORMATIONS  .  172 

C.  SCALING  AND  LIMITING  ROUTINES  .  175 

D.  TEST  RUN  RESULTS  .  176 

VIII.  CONCLUSIONS/AREAS  FOR  FURTHER  STUDY  .  220 


APPENDIX  A:  TRIGONOMETRY,  VECTORS,  AND  MATRICES 

A.1 :  TRIGONOMETRY  . 

A. 2:  MATRICES  . 

APPENDIX  B:  DENAVIT-HARTENBERG  NOTATION 

B. 1:  DENAVIT-HARTENBERG  NOTATION  . 

B.2:  GENERAL  ROBOT  D-H  MATRICES  . 

B.3:  ARTICULATED  ROBOT  D-H  MATRICES  . 

APPENDIX  C:  CARTESIAN  ROBOT  DSL  PROGRAMS  . 

APPENDIX  D:  ARTICULATED  ROBOT  DSL  PROGRAMS  . 

APPENDIX  E:  PATH  CONSTRUCTION  PROGRAMS  IN  FORTRAN 

E.1  ROBOT  PATH  PROGRAM  . 

E.2  LINEAR  PATH  1  MAIN  PROGRAM  . 

E.3  LINEAR  PATH  2  MAIN  PROGRAM  . 

E.4  CIRCULAR  PATH  MAIN  PROGRAM  . 

E.5  HELIX  PATH  1  AND  2  MAIN  PROGRAM  . 

E.6  LINEAR  PATH  MAIN  PROGRAM  . 

E.7  SINUSOIDAL  PATH  MAIN  PROGRAM  . 

APPENDIX  F:  3-DIMENSIONAL  PLOTTING  PROGRAM  . 

APPENDIX  G:  COORDINATE  TRANSFORMATION  PROGRAMS  .. 

APPENDIX  H:  MISCILLANEOUS  PLOTTINGS  . 

LIST  OF  REFERENCES  . 

BIBLIOGRAPHY  . 

INITIAL  DISTRIBUTION  LIST  . 


uwwuwuwuu  uwwiwm  n  u  ?  ■ 


r*  r**r% *  rw*  nr .  "vu  ww  w-r 


LIST  QF-TABLES 

2.1  LINK  PARAMETERS  FOR  3  D.O.F.  CARTESIAN  ROBOT  .  32 

3.1  PARAMETRIC  DATA  FOR  JOINT  SERVO  MOTORS  .  43 

4.1  DSL  MODIFICATIONS  AND  SIMULATIONS  .  52 

4.2  DSL  SINGLE  RUN  MODIFICATIONS  .  54 

6.1  TEST  RUNS  . 100 

6.2  CIRCULAR  MOTION  TEST  RUNS/  VELOCITY  METHOD  .  1 05 

6.3  ACCURACY  USING  POSITION  METHOD  .  109 

7. 1  ARTICULATED  ROBOT  ARM  PATH  MOVEMENT  .  1 77 


vf 


7 


1.1  CARTESIAN  ROBOT  MANIPULATOR  AND  SKELETON  DIAGRAM  .  26 

1.2  CYLINDRICAL  ROBOT  MANIPULATOR  AND  SKELETON  DIAGRAM  27 

1 .3  SPHERICAL  ROBOT  MANIPULATOR  AND  SKELETON  DIAGRAM  .  28 

1.4  ARTICULATED  ROBOT  MANIPULATOR  AND  SKELETON  DIAGRAM  ....  29 

1.5  LEVER  DIAGRAM  FOR  2-DIMENSIONAL  ARTICULATED  ROBOT  .  30 

2.1  SKELETON  DIAGRAM  FOR  CARTESIAN  ROBOT  .  39 

3.1  SECOND  ORDER  MODEL  FOR  THE  IDEALIZED  SERVO  MOTOR  .  46 

3.2  SECOND  ORDER  MODEL  JOINT  BLOCK  DIAGRAM  .  47 

4.1  0.1  STEP  EXCITED  DSL  RESULTS 

(a)  X.Y.  and  Z  STEP  RESPONSE  .  55 

(b)  X.Y.  and  Z  PHASE  PLANE  .  56 

(c)  XY  PLANE  PROJECTION  .  57 

(d)  XZ  PLANE  PROJECTION  .  58 

(e)  YZ  PLANE  PROJECTION  .  59 

4.2  0.1  RAMP  EXCITED  DSL  RESULTS  (TV1-.5*FINTIME) 

(a)  X,Y,  and  Z  STEP  RESPONSE  .  60 

(b)  X.Y,  and  Z  PHASE  PLANE  .  61 

(c)  XY  PLANE  PROJECTION  .  62 

(d)  XZ  PLANE  PROJECTION  .  63 

(e)  YZ  PLANE  PROJECTION  .  64 


rV.  ' 


4.3  0.1  RAMP  EXCITED  DSL  RESULTS(TV1=.66*FINTIME) 

(a)  X,Y,  and  Z  STEP  RESPONSE  .  65 

(b)  X,Y,  and  Z  PHASE  PLANE  .  66 

(c)  XY  PLANE  PROJECTION  .  67 

(d)  XZ  PLANE  PROJECTION  .  68 

(e)  YZ  PLANE  PROJECTION  .  69 

4.4  EXCITATION  EXAMPLES  OF  STEP  AND  RAMP  EXCITATION  .  70 

4.5  DSL  MODIFICATIONS  TO  ROBSIM2  FOR  MULTIPLE  RUNS  .  71 

4.6  DSL  MODIFICATIONS  TO  ROBSIM1, 2, 2N,  AND  3  .  72 

5.1  PATH  PROGRAM  MODIFICATION  REGIONS  .  81 

5.2  PROCEDURE  FOR  UPDATING  PATH  EQUATION  .  82 

5.3  ROBOT  PATH  FORMATION  FLOWCHART  [ROBPATH4. FORTRAN]  .  83 

5.4  HOME  TO  START  SUBROUTINE  FLOWCHART  .  85 

5.5  CARTESIAN  START  TO  FINISH  SUBROUTINE  FLOWCHART  .  87 

5.6  FINISH  TO  START  SUBROUTINE  FLOWCHART  .  90 

5.7  FINISH  TO  HOME  SUBROUTINE  FLOWCHART  .  92 

5.8  PARAMETRIC  START  TO  FINISH  SUBROUTINE  FLOWCHART  .  94 

6.1  PATH  EQUATIONS  AND  PROGRAM  MODIFICATIONS  . Ill 

6.2  LINEAR  PATH  1  RESULTS,  STEP  EXCITATION,  POSITION  METHOD 

(a)  X,  Y,  and  Z  TIME  RESPONSE  . 1 1 2 

(b)  XY,  XZ,  and  YZ  PLANE  PROJECTION  . 112 

(c)  X,  Y,  and  Z  PATH  ERROR  . 113 

(d)  3-DIMENSION  VIEW  OF  LINEAR  PATH  1  . 114 


6.3  LINEAR  PATH  2  RESULTS,  STEP  EXCITATION,  POSITION  METHOD 

(a)  X,Y,  and  Z  TIME  RESPONSE  .  115 

(b)  XY,  XZ,  and  YZ  PLANE  PROJECTION  .  115 

(c)  X,Y,  and  Z  PATH  ERROR  .  116 

(d)  3-DIMENSION  VIEW  OF  LINEAR  PATH  2  .  117 

6.4  CIRCULAR  PATH,  STEP  EXCITATION,  POSITION  METHOD 

(a)  X,Y,  and  Z  TIME  RESPONSE  .  118 

(b)  XY,XZ,  and  YZ  PLANE  PROJECTION  .  118 

(c)  X,Y,  and  Z  PATH  ERROR  .  119 

(d)  3-DIMENSION  VIEW  Oc  CIRCULAR  PATH  .  120 

6.5  CIRCULAR  PATH,  RAMP  EXCITATION,  POSITION  METHOD 

(a)  X,Y,  and  Z  TIME  RESPONSE  .  121 

(b)  XY,  XZ,  and  YZ  PLANE  PROJECTION  .  121 

(c)  X,Y,  and  Z  PATH  ERROR  .  122 

6.6  CIRCULAR  PATH,  STEP  EXCITATION,  VELOCITY  METHOD 

(a)  T-.020.XY  PLANE  PROJECTION/ X,Y,  and  Z  PATH  ERROR .  123 

(b)  T-.015,  XY  PLANE  PROJECTION/  X,Y,  and  Z  PATH  ERROR .  124 

(c)  T-.010,  XY  PLANE  PROJECTION/  X,Y,  and  Z  PATH  ERROR .  125 

(d)  T-.008,  XY  PLANE  PROJECTION/  X,Y,  and  Z  PATH  ERROR .  126 

(e)  T-.007,  XY  PLANE  PROJECTION/  X,Y,  and  Z  PATH  ERROR .  127 

(f)  T-.006,  XY  PLANE  PROJECTION/  X,Y,  and  Z  PATH  ERROR .  128 

(g)  T-.005,  XY  PLANE  PROJECTION/ X,Y,  and  Z  PATH  ERROR 129 

(h)  T-.004,  XY  PLANE  PROJECTION/ X,Y,  and  Z  PATH  ERROR 130 

(i)  T-.003,  XY  PLANE  PROJECTION/  X,Y,  and  Z  PATH  ERROR .  131 

0)  T-.002,  XY  PLANE  PROJECTION/ X,Y,  and  Z  PATH  ERROR .  132 

(k)  T-.001 ,  XY  PLANE  PROJECTION/  X,Y,  and  Z  PATH  ERROR .  133 

(l)  T-.00r,  XY  PLANE  PROJECTION/  X,Y,  and  Z  PATH  ERROR  ...  134 

6.7  CIRCULAR  PATH,  RAMP  EXCITATION,  VELOCITY  METHOD 

(a)  T-.020,  XY  PLANE  PROJECTION/  X,Y,  and  Z  PATH  ERROR .  135 

(b)  T=.015,  XY  PLANE  PROJECTION/ X,Y,  and  Z  PATH  ERROR .  136 

(c)  T-.010,  XY  PLANE  PROJECTION/  X,Y,  and  Z  PATH  ERROR .  137 

(d)  T-.008,  XY  PLANE  PROJECTION/  X,Y,  and  Z  PATH  ERROR .  138 

(e)  T-.007,  XY  PLANE  PROJECTION/ X,Y,  and  Z  PATH  ERROR .  139 


10 


(f)  T-.006,  XY  PLANE  PROJECTION/  X,Y,  and  Z  PATH  ERROR .  140 

(g)  T=.005,  XY  PLANE  PROJECTION/  X,Y,  and  Z  PATH  ERROR .  141 

(h)  TV004,  XY  PLANE  PROJECTION/  X,Y,  and  Z  PATH  ERROR .  142 

(i)  T-.003,  XY  PLANE  PROJECTION/ X,Y,  and  Z  PATH  ERROR .  143 

0)  T=.002,  XY  PLANE  PROJECTION/  X,Y,  and  Z  PATH  ERROR .  144 

(k)  T-.001,  XY  PLANE  PROJECTION/  X,Y,  and  Z  PATH  ERROR .  145 

6.8  VELOCITY  METHOD  STEP  AND  RAMP  EXCITATION  ERROR  COMPARISON 

(a)  STEP  ERROR .  146 

(b)  RAMP  ERROR .  147 

(c)  STEP  AND  RAMP  COMPARISON .  148 

6.9  PARAMETRIC  EQUATIONS  AND  PATH  PROGRAM  MODIFICATIONS 

FOR  RIGHT  CIRCULAR  HELIXS  1  AND  2 . 149 

6.1 0  RIGHT  CIRCULAR  HELIX  1 ,  STEP  EXCITATION,  POSITION  METHOD 

(a)  X,Y,  and  Z  TIME  RESPONSE .  150 

(b)  XY,  XZ,  and  YZ  PLANE  PROJECTION .  150 

(c)  X,  Y, and Z PATH  ERROR.... .  151 

(d)  3-DIMENSION  VIEW  OF  HELIX  PATH  1 .  152 

6.1 1  RIGHT  CIRCULAR  HELIX  1 ,  RAMP  EXCITATION,  POSITION  METHOD 

(a)  X,Y,  and  Z  TIME  RESPONSE . . .  153 

(b)  XY.XZ,  and  YZ  PLANE  PROJECTION .  153 

(c)  X,Y,  and  Z  PATH  ERROR .  154 

6.1 2  RIGHT  CIRCULAR  HELIX  2,  STEP  EXCITATION,  POSITION  METHOD 

(a)  X,Y,  and  Z  TIME  RESPONSE .  155 

(b)  XY,  XZ,  and  YZ  PLANE  PROJECTION .  155 

(c)  X,  Y,  and  Z  PATH  ERROR .  1 56 

(d)  3-DIMENSION  VIEW  OF  HELIX  PATH  2 .  1 57 

6.13  RIGHT  CIRCULAR  HELIX  2,  RAMP  EXCITATION,  POSITION  METHOD 

(a)  X,Y,  and  Z  TIME  RESPONSE .  158 

(b)  XY,  XZ,  and  YZ  PLANE  PROJECTION .  1 58 

(c)  X,  Y,  and  Z  PATH  ERROR .  1 59 


6.14  RIGHT  CIRCULAR  HELIX  1 ,  STEP  EXCITATION,  VELOCITY  METHOD 

T=.01 5,  XY  PLANE  PROJECTION/  X.Y,  and  Z  PATH  ERROR .  1 60 

6.1 5  RIGHT  CIRCULAR  HELIX  1 ,  RAMP  EXCITATION,  VELOCITY  METHOD 

(a)  T-.020,  XY  PLANE  PROJECTION/  X,Y,  and  Z  PATH  ERROR .  161 

(b)  T=.015,  XY  PLANE  PROJECTION/  X,Y,  and  Z  PATH  ERROR .  162 

(c)  T=.010,  XY  PLANE  PROJECTION/  X,Y,  and  Z  PATH  ERROR .  163 

6.16  RIGHT  CIRCULAR  HELIX  2,  STEP  EXCITATION,  VELOCITY  METHOD, 

T=.01 5,  XY  PLANE  PROJECTION/  X, Y,  and  Z  PATH  ERROR .  164 

6.1 7  RIGHT  CIRCULAR  HELIX  2,  RAMP  EXCITATION,  VELOCITY  METHOD, 

T«=.020,  XY  PLANE  PROJECTION/  X,Y,  and  Z  PATH  ERROR .  1 65 

6.18  RIGHT  CIRCULAR  HELIX  ERROR  COMPARISON .  166 

6.19  SINUSOIDAL,  STEP  EXCITATION,  POSITION  METHOD 

(a)  X,Y,  and  Z  TIME  RESPONSE .  167 

(b)  XY,XZ,  and  YZ  PLANE  PROJECTION.. .  167 

(c)  X,  Y,  and  Z  PATH  ERROR .  168 

(d)  3-DIMENSION  VIEW  OF  SINUSOIDAL  PATH .  169 

6.20  SINUSOIDAL,  RAMP  EXCITATION,  POSITION  METHOD 

(a)  X,Y,  and  Z  TIME  RESPONSE .  170 

(b)  XY.XZ,  and  YZ  PLANE  PROJECTION .  170 

(c)  X,Y,  and  Z  PATH  ERROR .  171 

7.1  RANGE  OF  MOTION 

(a)  CARTESIAN  ROBOT .  180 

(b)  SPHERICAL  AND  ARTICULATED  ROBOT .  180 

7.2  CARTESIAN  TO  SPHERICAL  COORDINATE  CONVERSION .  1 81 

7.3  SPHERICAL  TO  ARTICULATED  COORDINATE  TRANSFORMATION  .  1 82 

7.4  ARTICULATED  ROBOT  PATH,  TRANSFORMATION,  SCALING,  AND  LIMITING 

PROGRAMS  AND  SUBROUTINES .  183 


TOPCTm k'JU 7?  •J'JV  JVW 7° '■'  -' TO •; »7*7J ■; ’>  ■>' -7-7 V  VT- 


7.5  ARTICULATED  ROBOT  PATH  OUTLINE .  184 

7.6  ARTICULATED  ROBOT  LINEAR  PATH  1 

(a)  TIME  RESPONSE .  185 

(b)  PATH  ERROR .  186 

(c)  ORIGINAL  PATH  IN  3-DIMENSIONS .  187 

(d)  DIRECT  CONVERSION  PATH  IN  3-DIMENSIONS .  188 

(e)  SIMULATED  CONVERSION  PATH  IN  3-DIMENSIONS .  1 89 

7.7  ARTICULATED  ROBOT  LINEAR  PATH  2 

(a)  TIME  RESPONSE .  190 

(b)  PATH  ERROR .  191 

(c)  ORIGINAL  PATH  IN  3-DIMENSIONS .  192 

(d)  DIRECT  CONVERSION  PATH  IN  3-DIMENSIONS .  193 

(e)  SIMULATED  CONVERSION  PATH  IN  3-DIMENSIONS .  1 94 

7.8  ARTICULATED  ROBOT  CIRCULAR  PATH 

(a)  TIME  RESPONSE .  195 

(b)  PATH  ERROR .  196 

(c)  ORIGINAL  PATH  IN  3-DIMENSIONS .  197 

(d)  DIRECT  CONVERSION  PATH  IN  3-DIMENSIONS .  198 

(e)  SIMULATED  CONVERSION  PATH  IN  3-DIMENSIONS .  1 99 

7.9  ARTICULATED  ROBOT  HELICAL  PATH  1 

(a)  TIME  RESPONSE .  200 

(b)  PATH  ERROR .  201 

(c)  ORIGINAL  PATH  IN  3-DIMENSIONS .  202 

(d)  DIRECT  CONVERSION  PATH  IN  3-DIMENSIONS .  203 

(e)  SIMULATED  CONVERSION  PATH  IN  3-DIMENSIONS .  204 

7.1 0  ARTICULATED  ROBOT  HELICAL  PATH  2 

(a)  TIME  RESPONSE .  205 

(b)  PATH  ERROR .  206 

(c)  ORIGINAL  PATH  IN  3-DIMENSIONS .  207 

(d)  DIRECT  CONVERSION  PATH  IN  3-DIMENSIONS .  208 

(e)  SIMULATED  CONVERSION  PATH  IN  3-DIMENSIONS .  209 


13 


7.  1 1  ARTICULATED  ROBOT  LINEAR  PATH 

(a)  TIME  RESPONSE .  210 

(b)  PATH  ERROR .  211 

(c)  ORIGINAL  PATH  IN  3-DIMENSIONS .  212 

(d)  DIRECT  CONVERSION  PATH  IN  3-DIMENSIONS .  213 

(e)  SIMULATED  CONVERSION  PATH  IN  3-DIMENSIONS .  21 4 

7. 1 2  ARTICULATED  ROBOT  SINUSOIDAL  PATH 

(a)  TIME  RESPONSE .  215 

(b)  PATH  ERROR .  216 

(c)  ORIGINAL  PATH  IN  3-DIMENSIONS .  217 

(d)  DIRECT  CONVERSION  PATH  IN  3-DIMENSIONS .  218 

(e)  SIMULATED  CONVERSION  PATH  IN  3-DIMENSIONS .  219 

B.  1  ROTARY  AND  PRISMATIC  JOINTS .  229 

B.2  LINK  COORDINATES  AND  PARAMETERS .  230 

B.3  ARTICULATED  ROBOT  D-H  MATRICES .  231 

D  1  ARTICULATED  ROBOT  1 .0  RADIAN,  STEP  EXCITED,  Cl ,  C2,  AND  C3 
STEP  RESPONSE .  250 

D.2  ARTICULATED  ROBOT  1 .0  RADIAN,  STEP  EXCITED,  Cl ,  C2,  AND  C3 
PHASE  PLANE .  251 


H.1  CIRCULAR  PATH,  STEP  EXCITATION,  POSITION  METHOD 

X,  Y,  Z  TIME  RESPONSE  AND  XY,  XZ,  AND  YZ  PLANE  PROJECTION  .  306 


H.2  CIRCULAR  PATH,  RAMP  EXCITATION,  POSITION  METHOD 

X,  Y,  Z  TIME  RESPONSE  AND  XY,  XZ,  AND  YZ  PLANE  PROJECTION  .  307 


m '4_» ■!  4V|^ iv Lt'O'O 


CIRCULAR  PATH  (REDUCED),  STEP  EXCITATION,  VELOCITY  METHOD, 
XY  PLANE  PROJECTION  AND  X,  Y,  AND  Z  PATH  ERRORS 

(a)  VALMIN-  10.0  MILLI-IN .  308 

(b)  VALMIN- 5.0  MILLI-IN .  309 

(c)  VALMIN- 1.0  MILLI-IN .  310 

(d)  VALMIN- 0.5  MILLI-IN .  311 

(e)  VALMIN- 0.1  MILLI-IN .  312 

(f)  VALMIN- 0.05  MILLI-IN .  313 

(g)  VALMIN- 0.01  MILLI-IN .  314 

(h)  VALMIN- 0.005  MILLI-IN .  315 


1 


HELIX  PATH  1  (REDUCED),  STEP  EXCITATION,  VELOCITY  METHOD, 

XY  PLANE  PROJECTION  AND  X,  Y,  AND  Z  PATH  ERRORS 

(a)  VALMIN- 50.0  MILU-IN .  316 

(b)  VALMIN- 10.0  MILLI-IN .  317 

(c)  VALMIN- 5.0  MILLI-IN .  318 

(d)  VALMIN- 1.0  MILLI-IN .  319 

(e)  VALMIN- 0.5  MILLI-IN .  320 

(f)  VALMIN-  0.1  MILLI-IN .  321 

(g)  VALMIN- 0.05  MILLI-IN .  322 


t\ 


■->  ■  -  •  -  *  -  ’j1  *_■  ‘  j’  *- 


One  of  the  primary  concerns  with  the  use  of  robotics  to  do  tasks  is  for 
that  robot  to  perform  the  task  in  real  time  while  maintaining  control  over 
the  arm  with  the  minimum  of  interface  with  the  real  world.  Currently 
attempts  are  being  made  to  control  robots  with  elaborate  Artificial 
Intelligence  (Al)  systems  where  the  robot  responds  to  outside  stimuli  and 
uses  complex  algorithms  to  perform  the  next  motion.  A  drawback  to  this 
procedure  is  that  it  requires  a  large  amount  of  data  to  be  correlated  in  a 
short  period  of  time.  In  the  future  as  sensors  become  more  accurate  and 
computers  speed  up  so  that  the  data  can  be  handled  in  a  real  time  fashion 
then  the  approach  of  Al  will  be  effective.  Until  that  time  Robots  will 
require  pre-designed,  pre-determined  responses  to  make  the  required 
movement.  The  accuracy  of  these  motions  will  depend  on  the  techniques 
that  were  used  in  their  construction.  Three  methods  are  currently  being 
utilized  to  teach  the  Robot  to  perform  the  desired  task. 

A.  TEACHING  METHODS 

The  first  two  methods  used  in  teaching  the  Robot  are  Manual  Teaching 

and  Lead-Through  teaching.  Manual  teaching  is  sometimes  called 

teaching-by-showing  or  guiding  and  has  been  used  since  the  early  1950's. 

Probably  the  most  used  method  for  teaching  a  robot  to  move  in  the  desired 

patterns,  it  involves  manipulating  the  robot  arms,  manually,  while  the 

joint  coordinates  are  being  stored  corresponding  to  each  position.  Once 
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stored,  the  program  is  executed  causing  the  robot  to  move  through  the 
joint  vectors  in  the  specified  sequence.  This  method  does  not  require  a 
computer  which  makes  it  good  for  applications  that  are  repetitive  in 
nature  and  fairly  simple  such  as  spot  welding,  painting,  or  handling 
materials. 

Lead-through  teaching,  which  is  similar  to  manual  teaching,  is  the 
simplest  for  programming  continuous  path  (CP)  robot  systems.  One 
method  is  to  grab  the  manipulator  and  lead  it  through  the  entire  range  of 
motion  at  the  desired  speed,  while  recording  the  continuous  position  of 
each  axis.  Due  to  the  size  and  construction  of  some  robots  it  may  be 
necessary  to  have  an  identical  manipulator,  equipped  with  the  position 
sensors,  to  be  led  by  the  operator.  Sometimes  called  a  robot  simulator  or 
a  teaching  arm,  it  is  grasped  by  the  operator  and  led  through  the  range  of 
motion  required  while  the  coordinate  position  of  the  joints  are  being 
sampled  at  a  constant  frequency  and  stored  in  a  computer.  Disadvantages 
to  the  lead-through  teaching  method  includes  1)  requires  a  simulator;  2) 
unintentional  motions  will  be  recorded  and  then  played  back;  3)  since  it 
is  done  manually  high  precision  is  not  possible;  4)  it  is  impossible  to 
obtain  the  exact  required  velocity;  and  5)  large  memory  size  is  required. 

B.  PROGRAMMING 

The  third  method  used  to  teach  robots  to  perform  is  through  the  use  of 

programming.  To  perform  complex  maneuvers,  or  fairly  random 

maneuvers,  robots  need  to  be  under  the  control  of  some  form  of 

programming.  This  is  to  allow  sensors  to  be  positioned  in  the  correct 
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perspective,  or  data  to  be  retrieved.  There  are  several  levels  of 
programming  to  be  considered. 

Robot  level  programming.  At  this  level  the  computer  programming 
language  correlates  the  sensor  data  and  then  specifies  the  robot  motion. 
This  requires  that  the  programmer  be  familiar  with  not  only  the 
programming  language  but  with  techniques  used  in  sensor  guided  motion. 
There  are  several  types  of  robot  level  programming  systems  that  have 
been  around  since  the  early  sixties.  They  include  MHI  (1960-1961),  WAVE 
(1970-1975),  MINI  (1972-1976),  AL  (1974-present),  VAL 
(1975-PRESENT),  AML  (1977-PRESENT),  TEACH  (1975-1978),  PAL 
(1978-PRESENT),  MCL  (1979-PRESENT),  and  many  others  (Lozano-Perez, 
1983,  pp.  821-841 ;  Fu,  Gonzalas,  and  Lee,  1987,  pp.  450-473). 

Robot  motion  by  taking  the  basic  concept  of  guiding  and  incorporating  a 

decision  matrix  that  is  based  on  sensor  input.  All  of  the  programming 

systems  require  the  use  of  some  form  of  guiding.  Basic  guiding  is  the 

simplest  form  and  takes  a  sequence  of  robot  positions  and  repeats  them 

back  to  the  robot.  Most  of  the  basic  guiding  systems  are  specified  by  a) 

the  way  in  which  the  positions  are  specified,  or  b)  the  method  that  the 

robot  uses  to  get  from  one  position  to  the  next.  The  most  common  way  to 

specify  the  position  is  to  a)  use  a  teach-pendant,  or  b)  move  the  robot 

through  the  desired  sequence  of  motions  manually  or  by  using  some  sort  of 

master-slave  linkage.  When  using  the  teach-pendant  method,  only  a  few 

points  are  used  and  the  motion  is  usually  straight  line  between  two  points 

relative  to  a  coordinate  system.  Positions  in  between  are  interpolated 

based  on  the  coordinate  system  involved.  Extended  guiding  is  based  on 

18 


using  sensors  to  tell  the  position  of  an  object  and  basing  the  motion  of  the 
robot  relative  to  the  coordinate  system  of  that  object.  Off-line  guiding 
incorporates  the  task  model  with  the  robot  model  and  then  simulates  the 
motion  of  the  robot  in  response  to  a  program  or  guiding  input.  This  allows 
for  some  versatility  in  choosing  the  robot  path  and  allows  for  constraints, 
such  as  minimizing  time,  to  be  incorporated. 

Task  level  programming.  The  user  defines  the  task  and  the  robot 
calculates  the  necessary  parameters  to  perform  the  task.  This  makes  the 
robot  entirely  independent  of  the  user,  requiring  no  special  pre-defined 
positions  or  paths  that  depend  on  complex  geometrical  computations  to  be 
supplied  by  the  user.  Task  level  programming  is  accomplished  with  a 
task-planner,  which,  when  given  a  description  of  the  object  to  be 
manipulated,  initial  and  final  states,  and  a  description  of  the  robot 
carrying  out  the  task,  formulates  a  robot-level  program  designed  to 
accomplish  the  task.  Examples  of  task-level  systems  include  HAND-EYE 
[Stanford],  LAMA  [MIT],  AUTOPASS  [IBM],  RAPT,  and  ROBEX. 

C.  ROBOT  MODEL 

So  far  we  have  made  no  mention  of  the  Robot  or  the  type  of  simulation 
needed  to  accurately  predict  the  intended  motion  of  the  Robot.  Several 
authors  have  defined  the  type  and  classification  (Koren,  1983;  Coiffet, 
1983,  pp.1 1-37)  of  robot  models  that  must  be  considered  when  selecting  a 
Robot  or  simulation.  Most  of  the  Robots  are  classified  by  the  type  of 
coordinate  system  that  the  arms  move  in.  Of  these  there  are  four  main 
classifications: 
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a.  Linear  robot  model  (cartesian),  where  each  of  three  degrees  of 
freedom  move  independently,  and  in  a  linear  fashion.  The  advantages 
to  this  model  are  the  independence  of  the  degrees  of  freedom  reduce 
the  kinematics  equation  to  a  simple  level,  however  the  model  lacks 
mechanical  flexibility.  It  is  a  good  stepping  stone  to  the  more 
advanced  robot  models. 

b.  Cylindrical  coordinate  robot.  Consists  of  a  horizontal  arm  mounted  on 
a  vertical  column  which  is  attached  to  a  rotating  base.  The  horizontal 
arm  is  capable  of  moving  in  and  out  radially,  and  up  and  down  the 
column.  The  main  disadvantage  is  that  the  resolution  of  the 
movement  increases  as  a  function  of  radial  distance. 

c.  Spherical  coordinate  robots.  Similar  to  the  cylindrical  robot  however 
the  arm  is  placed  at  the  end  of  the  vertical  column  and  telescopes  in 
and  out.  Disadvantage  is  that  the  resolution  is  low  in  two  axes 
directions.  The  advantage  is  that  there  is  more  mechanical  flexibility 
and  the  Robot  can  access  areas  below  the  the  base  plane. 

d.  Articulated  or  Revolute  coordinate  system  where  all  three  of  the 
commanded  motions  are  in  terms  of  angles  (radians).  This  is 
preferable  to  the  others,  but  the  kinematics,  non-linearity  of  the 
servos,  and  time  varying  torques  on  each  arm,  make  it  a  lot  more 
complicated.  The  main  disadvantage  is  that  the  joint  errors 
accumulate  in  the  end  of  the  arm.  The  advantages  are  that  it  is  the 
most  flexible,  and  can  move  at  higher  speeds  than  the  first  three 
models. 
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Figure  1 .1  (a)-(d)  show  examples  of  each  of  the  coordinate  systems 
involved  along  with  skeleton  models  which  interpret  the  physical  motion 
as  a  mathematical  model.  It  is  easy  to  see  that  the  specific  task  a  robot 
is  to  be  used  for  will  be  a  big  factor  in  determining  the  type  of  robot  that 
is  needed. 

The  number  of  degrees  of  freedom  (d.o.f.)  determine  the  ability  of  the 
robot  to  cover  the  design  space.  The  most  common  d.o.f.  are  two,  three, 
six,  and  seven.  The  advantage  of  two  d.o.f.  is  in  the  simplicity  of  motion 
with  the  disadvantage  of  only  being  able  to  move  in  a  planar  or 
2-dimensional  coordinate  space  (2-D).  Three  d.o.f.  give  a  much  more 
desirable  motion  in  that  a  full  three  dimensional  space  can  be  considered. 
Most  applications  that  exist  will  require  as  much  of  3-dimensional  space 
as  possible  to  be  accessible  to  the  tip  of  the  robot.  Six  degrees  of 
freedom  is  the  desired  end  product,  which  consists  of  the  three  degree  of 
freedom  model  with  a  three  degree  of  freedom  manipulator  at  the  end  of 
its  arm.  This  allows  for  a  full  range  of  motion  similar  to  a  human  hand.  A 
lot  of  interest  is  placed  in  seven  d.o.f.  as  it  will  not  only  cover  the  same 
motion  as  six  d.o.f.  but  has  the  added  advantage  of  more  accurately 
representing  the  motion  of  the  human  body  and  allows  the  robot  more 
freedom  in  moving  from  one  position  to  the  next. 

Another  important  aspect  of  the  Robot  model  is  the  kinematic  and 
dynamic  equations  of  motion  used  to  calculate  the  movement  of  the  robot 
and  describe  the  dynamic  behavior  of  the  manipulator.  Several  methods 
are  available,  each  offering  a  different  viewpoint.  They  are: 

a.  Kinematics.  In  the  kinematics  equations  you  are  given  the  angles  of 
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the  arms  with  respect  to  the  coordinate  system  and  get  as  a  result  the 
position  of  the  end  manipulator. 

b.  Inverse  Kinematics.  The  inverse  kinematics  equation  starts  with  the 
position  of  the  end  manipulator  and  results  in  the  calculation  of  the 
angles  of  the  arms. 

c.  Lagrangian.  The  lagrangian  method  is  based  on  the  calculation  of  the 
kinetic  energy  and  the  potential  energy  of  the  manipulator  and  is  more 
analytic  in  nature. 

d.  Newton  Euler  Formulation.  A  numerical  method  using  the  center  of 
mass  theorem  (Newton's)  and  the  kinematic  theorem  (Euler's)  applied 
to  each  link  of  the  robot  manipulator. 

There  are  several  articles  and  textbooks  (Featherstone,  1983,  p.  35; 

Fu,  Gonzolas,  and  Lee,  1987;  Paul,  1981 ,  pp.  41-118;  Lee,  1981 ,  pp.  62-68; 
Lee  and  Ziegler,  1984,  pp.  695-696;  Lee  and  Chang,  1986,  pp.  1-4;  Brady, 
1982,  pp.  51-126)  which  go  into  detail  as  to  which  of  the  equation 
techniques  is  the  best  for  the  application  and  which  method  is  easier  and 
quicker  to  use.  Not  only  do  these  methods  apply  to  the  Robot  Model  but 
they  also  apply  to  the  way  that  the  path  of  the  robot  will  be  calculated. 

D.  PATH  APPLICATION 

Movement  of  the  Robot  requires  that  not  only  do  you  know  where  you 

want  to  go  but  that  you  know  where  you  have  been.  Starting  and  ending  a 

series  of  motions  from  a  pre-defined  "home"  position  allow  all  of  the  path 

motions  to  originate  from  a  known  point  with  a  higher  accuracy  than  if 

you  were  to  start  the  motion  at  the  last  known  point.  There  is  ,  however  a 
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disadvantage  in  that  the  arm  may  be  closer  to  the  next  desired  position 
and  there  may  be  a  significant  time  loss  in  moving  "home"  and  then  back  to 
commence  the  next  series  of  movements.  The  end  result  of  the  motion 
must  also  be  considered  when  determining  where  a  path  equation  is  to  go. 
You  need  to  determine  whether  or  not  the  paths  of  the  arms  and  all  of  the 
joints  need  to  be  programmed  or  just  the  tip  of  the  last  or  furthest  arm. 

That  tip  is  referred  to  as  the  endpoint. 

When  trying  to  avoid  objects  it  is  necessary  to  know  where  all  of  the 
arms  are  at  all  instants.  If  the  object  is  within  the  movement  space  then 
it  may  prevent  the  Robot  from  reaching  some  portions  of  the  workspace. 
Having  the  Robot  tip  blindly  go  to  a  region  where  there  is  no  object  does 
not  mean  that  another  part  of  the  Robot  may  not  hit  the  object.  If  some 
object  is  moving  in  the  workspace  and  must  be  avoided  by  the  Robot, 
computation  of  an  acceptable  path  for  the  Robot  arm  may  be  very  difficult. 

It  is  problems  like  these  that  are  driving  researchers  to  Al  techniques  and 
away  from  Robots  that  do  not  "think". 

E  MOTOR  CONTROL 

The  type  of  motor  control  used  can  be  either  open  loop  or  closed  loop. 
In  an  open  loop  control  system  the  accuracy  is  based  on  the  ability  of  the 
arm  sensors  to  match  the  desired  control  input.  This  is  critical  since  as 
the  load  and  arm  position  change  the  torque  required  of  the  motor  changes. 
In  the  closed  loop  system,  position,  velocity,  or  both  are  compared  with 
the  desired  values  and  the  error  is  fedback  to  the  controlling  circuitry  so 
that  the  error  is  reduced  to  minimum.  With  a  highly  accurate  position 


servo  the  type  of  control  becomes  less  and  less  a  factor.  Another  factor 
that  affects  the  accuracy  of  the  system  is  the  type  of  motor  drive.  The 
most  accurate  are  the  direct  coupled  drives  while  the  least  accurate 
indirectly  drive  the  arms  through  a  system  of  cables,  gears,  chains,  or 
screws.  The  type  of  motor  to  be  controlled  must  also  be  considered. 
Stepping  motors,  hydraulic  actuators,  dc  servo-motors,  or  pneumatic 
cylinders  each  present  unique  problems  in  relationship  to  the  overall 
system  and  path  design. 

Other  things  to  be  considered  depend  on  operating  specifications.  Do 
you  want  to  handle  the  problem  in  minimum  time  or  do  you  need  to 
conserve  energy  or  cost?  Is  track  accuracy  important  or  is  the  speed  of 
the  task  important?  unfortunately  there  is  no  single  answer  to  all  of 
these  problems  and  again  the  task  determines  the  design. 

F.  OBJECTIVES 

The  objective  of  this  thesis  is  to  have  a  Robot  arm  follow  a 
predetermined  path.  The  Path  will  be  defined  as  a  set  of  coordinate  points 
to  be  fed  into  a  program  which  simulates  a  Robot  arm.  That  is  a  path  that 
is  previously  calculated  so  that  all  that  needs  to  be  done  is  to  feed  in  the 
control  signals  to  the  Robot  arm. 

Chapter  Two  models  the  Robot  Manipulator  mathematically  without 

regard  to  the  motor  characteristics  and  considering  only  the  physical 

characteristics  of  the  arm  and  the  coordinate  space  that  the  arm  operates 

in.  Due  to  the  simplicity  a  Cartesian  Robot  was  chosen  to  perform  the 

movements.  Chapter  Three  takes  the  motor  and  derives  a  computer 
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simulation  model  to  be  used  in  conjunction  with  the  Robot  Manipulator  of 
Chapter  Two.  Chapter  Four  integrates  the  chapters  into  a  Dynamic 
Simulation  Language  (DSL)  program  and  tests  the  program  in  single  step 
movements  for  proper  operation  and  timing.  Chapter  Five  develops  a  path 
following  routine  Which  takes  cartesian  and  parametric  equations  and 
generates  a  list  of  three  tuples  which  are  the  desired  path  for  the 
Cartesian  Robot  to  follow.  Chapter  Six  adjusts  the  earlier  Robot 
simulation,  adding  control  procedures,  allowing  the  path  three  tuples  to  be 
automatically  entered  into  the  simulation  and  demonstrates  the  Robots 
ability  to  follow  the  generated  path.  Linear,  circular,  helical,  and 
sinusoidal  motion  are  examined  for  proper  operation.  Also  error  analysis 
and  criteria  for  proper  operation  are  discussed. 

Chapter  Seven  takes  the  same  path  model  for  the  Cartesian  Robot  and 
translates  to  the  "Articulated"  or  "revolute"  coordinate  system  for  the 
second  model  to  be  run.  Finally  Chapter  Eight  discusses  ideas  and 
directions  to  be  considered  for  future  work. 


Figure  1 A  Articulated  Robot  Model  and  Skeleton  Structure. 


A.  INTRODUCTION 


Two  Robot  manipulators  are  being  considered.  The  first  model  consists 
of  a  cartesian  robot,  while  the  second  model  consists  of  a  articulated  (or 
revolute)  robot.  Each  mathematical  model  will  be  configured  with  3  degrees 
of  freedom  (d.o.f.)  and  will  be  developed  using  lagrangian  equations  for  both 
the  dynamic  and  static  cases.  Parameters  of  interest  include:  position, 
velocity,  acceleration,  torque,  mass,  load  capabilities,  and  gravity.  The 
lagrangian  relates  the  the  parameters  of  each  joint  with  all  of  the  other 
joints.  Once  the  lagrangian  has  been  found  for  each  of  the  joints,  then  the 
equations  will  be  related  to  the  second  order  model  for  the  idealized  servo 
motor.  The  cartesian  models  are  being  considered  first  due  to  the  simplicity 
of  the  lagrangian  equations  and  because  it  will  be  beneficial  in  understanding 
the  relationships  in  the  articulated  robot. 

B.  CARTESIAN  ROBOT  MODEL 

The  3  d.o.f.  cartesian  robot  that  is  being  considered  is  shown  in  Figure 
1 .1  and  whose  skeletal  structure  is  displayed  in  Figure  2.1 .  The  motion  of 
the  arms  are  in  the  XYZ  coordinate  space  and  each  of  the  arms  moves  in  a 
linear  (prismatic)  fashion  and  there  are  no  revolute  joints.  Because  of  this 
they  can  permit  only  parallel  motion  and  have  no  rotation  vectors.  Using 
Denavit-Hartenberg  Notation  (appendix  A)  (  Lee,  1982,  pp.  86-70;  Fu, 
Gonzolas,  and  Lee,  1987,  pp.  36-41)  on  Figure  2.1  we  have  the  following 
transformation  matrices  described  by  link  parameters  in  Table  2.1 . 


TABLE  2.1 

LINK  PARAMETERS  FOR  3  D.O.F.  CARTESIAN  ROBOT 


Variable 

d, 


cos  a 
1 


sin  a 
0 


1  0  0  d, 
0  10  0 
0  0  10 
0  0  0  1 


10  0  0 
0  1  0  d2 
0  0  10 
0  0  0  1 


10  0  0 
0  10  0 
0  0  1  dj 

0  0  0  1 


[eqn  2.1  ] 


[eqn  2.  2  ] 


[eqn  2.  3  1 


Computing  °T3  from  equations  2.1  to  2.3  [appendix  A]  we  get 


1  0  0  d, 
0  1  0  d2 
00  I  d3 
0  0  0  1 


[eqn  2.  4  ] 


-  -V.V.V.V.  ^ -  o' 


which  is  the  description  of  the  end  of  the  manipulator  with  respect  to  the 
base.  If  the  manipulator  is  referenced  to  a  coordinate  system  by  a 
transformation  C  with  a  tool  described  by  E,  the  position  and  orientation  of 
the  tool  with  respect  to  the  coordinate  reference  is  described  by  X  as 

X  -  Z  °T3  E.  [eqn  2.5] 

C.  DERIVATION  OF  THE  LAGRANGIAN  EQUATION  OF  MOTION 

Lagrangian  formulation  of  manipulator  dynamics  describes  the  model  in 
terms  of  work  and  energy.  These  dynamic  equations  can  be  derived  in  any 
coordinate  system  and  are  easier  to  express  and  use  than  the  Newton-Euler 
formulation  [ASADA].  The  lagrangian  is  defined  as  the  the  difference 
between  the  kinetic  and  potential  energies  of  the  system 

L  -  K  -  P  [eqn  2.6] 

Once  the  lagrangian  has  been  obtained  for  each  of  the  joints  then  the 
dynamics  equation  is  formulated  as 

Fi-  £2L _ _2L  i-1...n  {eqn  2.7] 

dt9£Lflj  9qj 

d  t 

where  qj  are  in  the  coordinates  in  which  the  kinetic  and  potential  energy 
are  expressed,  dq\/dt  is  the  velocity,  and  Fj  is  the  torque  or  force, 
depending  on  whether  qj  is  in  a  linear  or  angular  coordinate  system.  The 
lagrangian  of  the  system  is  described  as  L  for  each  degree  of  freedom  n, 


that  is  to  be  modeled. 


For  the  3  d.o.f.  cartesian  coordinate  system,  expressed  in  cartesian 
coordinates,  all  of  the  joints  are  linear  so  that  the  dynamics  equation  is 
expressed  in  terms  of  forces,  distance  dj,  velocity  ddj/dt,  and 

acceleration  d  2  dj/cft2.  For  this  problem  the  lengths  of  the  manipulator 
arms  are  identical,  with  the  mass  of  each  arm  located  at  the  end  of  its  link. 

First  Node: 

(1/2)  m,  (dd^dt)2  [eqn  2.8] 

and  P1 »  -  m1  g  d1  -  0  [eqn  2.9] 

To  relate  the  second  node  to  the  first  it  is  necessary  to  first  write  the 
terms  for  the  position  and  then  taking  the  derivative  find  the  velocity 
components. 

Second  Node: 


x2  «  d, 
y2  *  d2 
z2  *  0 


[eqn  2.10] 
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where 


dx^d t«  dd,/dt 
dy^dt-  dd2/dt 
dz^d  t-  0 
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[v2]2  =[  dx^  d  t  ]2  +  [  dyg/  dt  ]2  +[  dz^  dt  ]2 

[eqn  2.11] 

K2=  (1/2)  mt{(d  d,/  d  t)2+  (d  d^/  d  t )2} 

[eqn  2.12] 

and  P2  -  -m2  g  d2  =  0 

[eqn  2.13] 

Third  node: 

x3  *  d,  dx^d t=*  dd,/dt 

y3  *  d2  dy^dt*  dd2/dt 

z3  «  d3  dZg/dt*  dd3/dt 

[eqn  2.14] 

where 

[v3]2  =[  dx3/  dt  ]2  +  [  dy3/  dt  ]2  +[  dz3l  dt  ]2 

[eqn  2.15] 

K3=  (1/2)01,  {(d  d,/d  t)2+(d  d^d  t)2+(d  d3/d  t)2} 

[eqn  2.16] 

and  P3  =  -  m3  g  d3 

[eqn  2.17] 

Combining  equations  [2.8],  [2.9],  [2.12],  [2.13],  [2.16],  and  [2.1 7]  into 
equation  [2.6],  and  solving  we  arrive  at  the  lagrangian 

L={1/2}[(  m1+m2+m3)(d  d,/d  t)2  +(  m2+m3 )( d  d ^d  t)2 

+  (  m3 )( d  d3/d  t  )2  ]  +  m3gd3  [eqn  2.18] 


and  calculating  the  dynamics  equation  for  each  node  from  equation  [2.18]  we 

have 

First  node: 

auatdd^dt]  =  (  m1+m2+m3  )(d  d^d  t) 

[eqn  2.19] 

d(  3L75[dd1/dt])/dt  *  (  n^+rrig+mg  )(d  2dt/d  t2) 

[eqn  2.20] 

a  L/3  d,  =0 

[eqn  2.21] 

we  arrive  at 

F1  «  (  n^+mg+mg  )(d  26ytd  t2 ) 

[eqn  2.22] 

Second  node: 

auafddg/dt]  *  (  m2+m3  )(d  d^d  t) 

[eqn  2.23] 

d  ( ai_/3[d dg/d t])/d t  =  ( m2+m3  )(d  ^d  t2 ) 

[eqn  2.24] 

a  L/a  d2  -  0 

[eqn  2.25] 

we  arrive  at 

F2  =  (  m2+m3  )(d  2dg/d  t2 ) 

[eqn  2.26] 

Third  Node: 
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8L/d[c/ dg/c/ 1]  =  (  m3)(d  d 3/d  t) 

d(dUd[dd3/d\])/dt  =  ( m3)(d  %/d  t2) 

d  Ud  d3  »  m3  g 
we  arrive  at 

F3  =  (  m2+m3  )(d  2d^d  t2 )  -  m3  g 


[eqn  2.27] 


[eqn  2.28] 


[eqn  2.29] 


[eqn  2.30] 


Equations  for  F,  [2.22],  F2  [2.26],  and  F3  [2.30],  are  the  lagrangian  dynamic 

equations  that  we  will  relate  to  the  servo  motor  simulation.  The  values 
selected  for  the  simulations  are 


1  unit 
1  unit 
1  unit 


m1 

* 

.082 

oz  +2mm  + 

load 

nrig 

SB 

.041 

oz  +  mm  + 

load 

m3 

S 

.041 

oz 

Q 

SB 

386.4 

in/sec2 

mm 

as 

0.186 

load 

s 

0.0 

A  quick  review  of  Figure  2.1  indicates  that  other  variations  are 
available  as  well.  In  the  calculations  we  assumed  that  the  arm  which 
allowed  X  direction  of  motion  actually  supports  and  moves  the  arms  for  Y 
and  Z  movement.  Also  the  arm  which  allowed  Y  motion  supported  the  arm 
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A.  INTRODUCTION 

The  DC  servo  motor  that  is  being  simulated  has  been  demonstrated 
(Thaler,  1956,  pp.  410-417;  Ozaslan,  1986,  pp.14-15)  to  have  the  transfer 
function 


where 


£&  =  _ 1Z& _ 

V(s)  S(S(JR/KvKt)+1)(SL/R+1) 


0(S) 

=  Angular  position  of  the  shaft 

V(s) 

=  Applied  d-c  voltage 

Kv 

■  Back  emf  constant 

Kt 

=  Torque  constant 

J 

=  Total  inertia 

R 

=  Armature  resistance 

L 

=  Armature  inductance 

[eqn  3.1] 


Since  the  robot  arm  has  a  large  inertia  when  added  to  the  motor  inertia 
this  causes  the  mechanical  pole  of  the  motor  to  become  smaller.  Coupled 
with  the  R/L  term,  which  is  large,  we  can  ignore  the  electrical  pole  and 
the  transfer  function  for  the  motor  and  robot  arm  is  approximately 

Qfs)  =  Km  leqn  3.2] 

V(s)  s2 


With  this  simplification  it  is  easy  to  see  that  Figure  3.1  is  the  block 
diagram  of  the  second  order  motor  model.  As  can  be  seen  we  are  using  a 


velocity  feedback  loop,  an  idealized  deceleration  curve  for  the  motor,  and 
Bang-Bang  control  to  drive  the  system.  Figure  3.2  ties  the  computer 
simulation  model  to  that  of  the  idealized  motor. 

B.  COMPUTER  MODEL 

The  computer  model  uses  position  feedback  while  the  idealized  motor 
uses  velocity  feedback  of  the  motor.  The  input  parameter  to  the  computer 
model  is  the  desired  position  which  drives  the  ideal  motor  with  the 
simulated  velocity,  V.  When  the  E  of  the  computer  model  has  dropped 
below  a  minimum  value  then  the  arm  is  in  the  desired  position.  Since  the 
deceleration  curve  of  an  ideal  motor  is  similar  to  a  parabolic  curve  it  was 
chosen  as  the  curve  to  be  simulated  in  the  DSL  program.  The  following 
equations  describe  the  parameters  outlined  in  Figure  3.2  for  the  computer 
model. 


C"  =  Km  *  Vsat 

[eqn  3.3] 

C’  -  B  C"  dt  =  Km*Vsat*t  +  C'(0) 

[eqn  3.4] 

C  =  B  C'  dt  *  (Km*Vsat*t2)/2  +  C(0) 

[eqn  3.5] 

where  C(0)  =  0.  Combining  equations  3.4  and  3.5  and  solving  for  C  we  find 
the  relationship 


C  =  (C')2/(2*Km*Vsat) 


[eqn  3.6] 


Other  relationships  include  ( for  X-arm  only): 


E  *  XPOS  -  C  [eqn  3.7] 

X’  =  -  A*K1*(ABS[E])1/2  [eqn  3.8] 

A  =  (2*Km*VSAT)  [eqn  3.9] 

where 

XPOS  is  the  desired  movement  in  X  direction 
X’  is  the  commanded  velocity 
Km  is  the  gain  constant : 

Kml  *  59.29 
Km2  =  90.25 
Km3  -  77.44 

K1  is  the  curve  scaling  constant  [0.6] 


X'E  «  X'  -  K*C'  [eqn  3.10] 

V  =  UMIT(Vsat,-Vsat,K2*X'E)  [eqn  3.1 1] 


where 

K  is  the  velocity  loop  feedback  gain  [computer  model][1 .0] 
K2  is  the  amplifier  gain  [10000.0] 

Vsat  [1 50  Volts] 
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The  value  of  K2  was  chosen  so  that  the  amplifier  saturates  for  small 
signals  with  full  values  of  forward  and  reverse  drive  signals  [±Vsat]. 


C.  SERVOMOTOR 

The  second  order  computer  model  drives  a  closed  loop-servo  for  each 
joint  of  the  robot  arm.  The  motor  used  to  drive  each  of  the  robot  arms  is  a 
permanent  magnet  motor  drive  with  Table  3.1  listing  the  parametric 
characteristics  (Ozaslan,  1986,  pp.  16-30). 

TABLE  3.1 

PARAMETRIC  DATA  FOR  JOINT  SERVO  MOTORS 


Torque  Constant 

Kt 

14.4 

oz-in/amp 

Total  Inertia  Jm 

0.033 

oz-in-sec2/rad 

Damping  coefficient 

Bm 

0.04297 

oz-in-sec/rad 

Back  EMF  Constant 

Kv 

0.1012 

volts-sec/rad 

Armature  Inductance 

L 

100 

pi-henries 

Avg  Terminal  Resistance 

R 

0.91 

ohms 

The  same  servo  drive  unit  will  be  used  for  all  of  the  joints.  Since  we  have 
only  linear  motion  at  the  joints  the  effective  inertia  is  only  the  servo 
motor  inertia.  For  the  cartesian  robot  system  there  is  no  reaction  torque 
(no  angular  acceleration),  no  centripetal  forces  (no  angular  velocity),  and 
no  coriolis  forces.  For  each  joint  we  have 


JTOT1  =  J1  +0.5*(M1  +  M2  +  M3  +  2*MM  +  LOAD)  [eqn  3.12] 

JTOT2  =  J2  +  0.5*(M2  +  LOAD)  [eqn  3.13] 


JT0T3  =  J3  +  0.5*(M2  +  M3  +  MM  +  LOAD) 


[eqn  3.14] 


where  J1 ,  J2,  and  J3  are  the  motor  drive  inertias  listed  in  Table  3.1 .  The 
mass  values  are 


Ml  =  0.082 
M2  =  0.041 
M3  =  0.041 
MM  =  0.186 
LOAD  -  0.0 


Applying  the  known  values  to  the  open  loop  equation  [eqn  3.1]  we  arrived  at 
the  values  for  KM1 ,  KM2,  and  KM3. 


D.  SERVO  MOTOR  SIMULATION  MODEL 

Taking  into  consideration  the  servo  motor  and  computer  model  values 
we  can  mathematically  model  the  servo  motor  with  the  following 
equations. 


CR"  =  MTE/JTOT  [eqn  3.15] 

CR’  =  B  CR"  dt  =  MTE/JTOT  *t  +  C’(0)  [eqn  3.16] 

CR  =  B  CR'  dt  =  ((  MTE/JTOT)  *t2)/2  +  C(0)  [eqn  3.17] 

where  C(0)  =  0.  Combining  equations  3. 16  and  3.17  and  solving  for  C  we 
find  the  relationship 
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CR  =  (CR')2/(2*  MTE/JTOT) 


[eqn  3.18] 


Other  relationships  include  ( for  single  motor  only): 


Vm  =  V  -  Kv*CR' 


where 


[eqn  3.19] 


[eqn  3.21] 


[eqn  3.22] 


MP  =  REALPL(0.0,L/R,Vm/R)  [eqn  3.20] 

MT  =  Kt*MP  [eqn  3 

MTE  =  MT  -  Bm*CR'  -  TL  [eqn  3 

V  is  calculated  in  the  computer  model 
Kv,  Bm,  Kt  are  from  Table  3.1 

TL  is  the  sum  of  centripetal  Torques,  Centrifugal  Torques, 
Reaction  Torques,  and  Gravitational  Torques  which  is 
zero  for  the  cartesian  robot  arm. 
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A.  INTRODUCTION 

In  Chapter  Three  the  computer  model  of  a  robot  servo  arm  was 
presented,  while  in  Chapter  Two  a  mathematical  model  was  constructed 
tying  the  three  robot  arms  together  in  a  cartesian  coordinate  system.  This 
chapter  takes  the  results  of  the  previous  chapters  and  generates  a  Dynamic 
Simulation  Language  [DSL]  Program  which  will  be  used  to  model  the  entire 
3  d.o.f.  Cartesian  Robot.  The  input  into  the  DSL  program  will  be  a  path 
described  by  a  three  tuple  of  coordinates.  The  program  simulates  the  robot 
movement  and  the  next  set  of  coordinate  points  are  inputted.  The  output  of 
the  DSL  Program  is  a  listing  of  the  inputted  and  simulated  values  based  on 
the  method  used  to  input  the  values. 

B.  PHILOSOPHY  OF  OPERATION 

The  method  used  to  input  the  data  along  with  the  control  parameters 
of  the  DSL  Program  determine  how  the  robot  will  perform  and  respond  to 
the  entered  coordinate  points.  The  two  methods  that  are  to  be  considered 
are  the  Position  Method  and  the  Velocity  Method.  The  Position  Method  is 
where  the  control  functions  of  the  DSL  Program  are  controlled  by  the 
accuracy  of  the  output  position  and  are  independent  of  time.  The  Velocity 
Method  fixes  the  time  interval  used  to  input  each  of  the  three  tuple 
coordinates  and  since  the  input  is  position  oriented  we  have  in  effect  a 
velocity  control. 
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The  Position  Method  calculates  the  robot  arm  endpoint  until  the 
voltages  for  all  of  the  arms  drops  to  below  a  stipulated  value.  This 
indicates  that  all  of  the  arms  are  within  the  desired  values  of  accuracy. 

The  advantages  of  the  Position  Method  show  up  in  the  accuracy  of  the  final 
position  of  each  move.  Since  the  step  size  is  predetermined  by  the 
programmer  and  the  accuracy  of  each  step  is  less  then  the  step  size  then 
the  overall  error  has  a  definite  maximum  limit.  The  disadvantage  with  the 
Position  Method  is  that  the  values  will  tend  to  be  very  accurate  at  the 
expense  of  time.  The  values  will  oscillate  around  the  desired  points  until 
all  of  the  voltage  values  are  within  a  certain  minimum  value  [VALMIN]  no 
matter  how  long  it  takes. 

The  Velocity  Method  on  the  other  hand  follows  the  same  trajectory 
but  when  the  time  interval  that  was  predetermined  has  been  reached  the 
next  three  tuple  of  coordinate  points  are  entered.  By  selecting  the 
maximum  velocity  available  for  a  given  step,  the  time,  determined  by  the 
DSL  program,  and  desired  length  for  each  step,  determined  by  the  computer 
path  program,  can  be  adjusted  to  maximize  the  performance  at  each  step. 
With  this  adaptability  the  user  can  optimize  the  overall  time  that  it  takes 
to  perform  the  journey  or  the  user  can  wait  at  each  point.  The  advantage 
of  this  method  is  in  the  versatility  in  controlling  with  either  time,  length 
of  travel,  or  a  mixture  of  both.  The  disadvantage  is  in  the  loss  of  accuracy 
at  the  desired  positions  but  with  proper  selection  of  time  and  length  this 
can  be  minimized. 


C.  EXCITATION  PROPOSALS 


Along  with  the  selection  of  the  method  to  which  the  DSL  Program  is 
organized  there  is  the  type  of  excitation  used  to  input  the  desired  position. 
This  excitation  comes  in  several  forms  of  which  the  three  of  interest  are 
the  STEP,  RAMP,  or  a  function  such  as  the  modified  SINE  wave.  This 
excitation  can  also  be  delayed  in  time  or  be  a  mixture  of  any  of  the 
excitations  as  desired.  Samples  of  the  STEP,  Figures  4.1  (a)-(e),  and  RAMP 
Excitations,  Figures  4.2  (a)-(e)  and  4.3  (a)-(e),  show  differences  in  the 
overall  response  for  a  single  inputted  position  step  of  0.1  units  to  all  three 
coordinates.  The  single  position  change  is  independent  of  the  time  base 
mentioned  earlier  but  will  give  an  idea  of  the  time  required  to  perform 
that  position  change,  and  the  accuracies  involved  in  moving  from  point  to 
point.  Figure  4.4  shows  three  different  excitation  techniques  used  to  apply 
the  movement  excitation  to  the  DSL  Program.  First,  the  STEP  excitation  is 
the  simplest  way  to  apply  the  movement  to  the  DSL  program.  This  does  not 
however  give  a  smooth  linear  response  from  the  simulated  robot  arm.  As 
seen  in  Figure  4.2  (c)-(e)  the  arm  movement  as  simulated  comes  out  fairly 
non-linear  but  is  still  within  our  earlier  assumption  that  the  maximum 
error  for  each  step  is  less  than  the  length  of  the  step  and  looks  to  be  about 
a  tenth  of  the  step  size  in  this  case. 

The  second  and  third  proposed  excitations  are  RAMP  excitations  and 

are  defined  by  DSL  to  be  the  sum  of  two  RAMPs,  the  first  RAMP  starting  at 

time  zero  (DSL  TIME  function),  and  the  second  RAMP  starting  at  time  TV1 

but  negative  in  value  with  the  same  slope  magnitude  as  the  first  RAMP. 

This  gives  a  RAMP  from  time  zero  to  TV1  with  the  magnitude  at  TV1  equal 
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to  the  same  magnitude  as  the  STEP  input.  Two  separate  TVI's  were  chosen 
to  see  what  the  overall  effects  would  be  on  the  simulation  arm  movement. 

Comparing  the  results  of  a  0.1  unit  position  increment,  in  all  three 
coordinate  directions,  when  excited  by  a  STEP,  Figure  4.1(a),  and  RAMPs, 
Figure  4.2(a)  and  4.3  (a)],  show  that  there  is  an  increase  in  time  for  the 
RAMP  to  complete  its  movement  and  arrive  at  the  desired  endpoint. 
However,  while  the  time  increased  for  the  RAMP  excitation  there  is  a 
reduction  in  the  arms  velocity  as  seen  in  Figures  4.1(b),  4.2(b),  and  4.3(b), 
and  the  linearity  of  the  arm  motion  between  desired  points  is  much  better. 
RAMP  excitations  provide  much  more  linear  response  than  STEP  excitations 
and  the  longer  TV1  is  then  the  more  accurate  a  RAMP  excitation  is.  For 
applications  where  accuracy  is  desired  it  may  be  more  beneficial  to  use  a 
RAMP  excitation  with  a  large  TV1 .  Use  of  TV1  instead  of  using  a  RAMP  for 
the  entire  step,  reduces  the  complexity  of  the  input  structure  and  attempts 
to  optimize  the  ramp  for  both  short  and  long  steps.  Although  most  of  the 
steps  are  nearly  the  same  length  there  are  times  when  movement  in  one  of 
the  directions  is  short  and  requires  a  faster  ramp  to  complete  it  in  average 
time.  TV1  was  chosen  as  an  attempt  to  keep  the  speed  up  while  sevicing 
both  long  and  short  steps. 

Close  examination  of  the  two  RAMP  excitations  show  that  the  longer 

the  RAMP,  relative  to  the  time  of  completion  desired,  the  more  linear  the 

travel  between  points  becomes  but  the  longer  it  takes  to  get  there  which 

is  due  to  the  slower  velocities  that  the  arms  peak  at.  When  designing  a 

system  for  operation  it  would  be  a  good  idea  to  incorporate  a  RAMP 

technique  to  lower  the  velocities  of  the  arms  and  reduce  torques  and 
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inertias  placed  on  the  motor  units  along  with  the  added  benefit  of  being 
more  accurate  between  the  inputted  steps. 

D.  MULTIPLE  RUNS 

The  DSL  Programming  Language  can  be  modified  to  tie  together  all  of 
the  concepts  that  have  been  discussed  in  this  chapter.  Some  adaptations 
are  necessary  to  simulate  each  of  the  methods.  Appendix  D  contains  a  copy 
of  all  of  the  modifications  used  to  calculate  all  of  the  different  DSL 
simulations  that  were  used  in  this  thesis.  Table  4.1  lists  all  of  the 
modifications  that  were  needed  and  their  location  in  Appendix  D. 


TABLE  4.1 

DSL  MODIFICATIONS  AND  SIMULATIONS 


PROGRAM 

EXCITATION 

LOCATION 

ROBSIM1 

STEP 

APP.  D-1 

ROBSIM2T 

POSITION 

STEP 

RAMP 

APP.  D-3 
APP.  D-3 

ROBSIM2S 

VELOCITY 

STEP 

RAMP 

APP.  D-4 
APP.  D-4 

The  only  difference  between  the  STEP  and  RAMP  excitation  programs 

are  in  the  way  the  data  points  are  applied  to  the  DSL  simulation  within  the 

DSL  program.  Figure  4.4  shows  a  sample  of  the  STEP  and  RAMP  Excitations 

along  with  the  equations  required  to  adapt  the  DSL  program  for  the  Ramp 

Excitation.  Figure  4.5  lists  the  modifications  that  are  needed  to  change 

the  DSL  program  from  the  Position  Method  of  operation  to  the  Velocity 

Method.  Actual  test  runs  will  be  conducted  and  referenced  in  Chapter  Six. 
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E.  TEST  RUNS 

To  insure  the  proper  operation  of  the  DSL  program  it  is  necessary  to 
make  a  series  of  single  runs  to  check  on  timing  and  operation  for  steps  of 
various  sizes  and  negative  steps.  Table  4.2  lists  the  DSL  programs  that 
were  used  to  test  the  Robot  Model  DSL  program  and  shows  the  step  sizes 
and  the  time  that  it  took  each  arm  to  complete  the  desired  step.  Figure 
4.6  shows  the  modifications  that  were  made  to  ROBSIM1  to  make  the  test 
runs.  For  each  direction  of  each  step  a  plot  of  velocity  vs.  distance  and  a 
plot  of  distance  vs  time  was  done.  Due  to  the  number  of  plots  that  were 
done  only  a  representative  set  of  plots  are  shown. 

All  of  the  DSL  simulations  should  give  the  same  time  response  the 
averages  for  each  of  the  step  sizes.  ROBSIM2  was  chosen  as  the  base  for 
the  multiple  run  simulations  was  due  to  the  control  statement  and  graph 
ranges. 

As  can  be  seen  from  Table  4.2  there  is  a  distinct  relationship  between 
the  length  of  the  step  size  and  the  time  that  it  takes  the  robot  simulation 
to  reach  that  value.  We  will  use  this  relationship  when  deciding  the  time 
that  will  be  used  in  the  Velocity  Multiple  Run  simulations  to  set  up  the 
time  interval  for  repetitive  data  input  and  control  FINTIME  for  DSL. 
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TABLE  4.2 

DSL  SINGLE  RUN  MODIFICATIONS 


DSL 

STEP  SIZE 

TIME 

f10'3-SECS) 

PBQGBAM 

(UNITS) 

X 

X 

z 

ROBSIM1 

1.00 

40.5 

30.0 

34.0 

0.50 

28.0 

20.5 

24.0 

0.20 

19.0 

14,0 

15.0 

0.10 

10.2 

08.5 

10.0 

0.05 

09.0 

06.0 

06.5 

0.02 

05.0 

03.0 

04.0 

ROBSIM2 

0.10 

13.0 

09.5 

11.0 

0.05 

09.0 

07.0 

08.5 

0.02 

06.0 

04.0 

05.0 

0.01 

04.0 

03.0 

03.0 

ROBSIM3 

0.02 

06.0 

04.2 

05.0 

0.01 

04.2 

03.0 

03.5 

0.005 

03.0 

02.2 

02.5 

ROBSIM2N 

-0.10 

13.0 

10.0 

10.5 

-0.05 

09.0 

06.5 

07.5 

-0.02 

06.0 

04.5 

05.0 
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STEP  RESPONSE 


Figure  4.1(a)  0.1  STEP  Excited  X,  Y,  and  Z  Step  Response 


Figure  4  1(d)  0  1  STEP  Excited  XZ  Plane  Projection. 
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Figure  4.2(a)  0.1  RAMP  Excited  X,  Y,  and  Z  Step  Response. 
[TV  1  =  0.5*FINTIME). 
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Figure  4  2(b)  0.1  RAMP  Excited  X,  Y,  and  Z  Phase  Plane. 
[TV  1  =  0  5*FINTIME). 
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Figure  4.2(c)  0  I  RAMP  Excited  XV  Plane  Projection 
(TV  1  =  0.5*FINTIME). 
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Figure  4.3(e)  0. 1  RAMP  Excited  Y2  Plane  Projection. 
[TV  1  =  0  66*FINTIME). 
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DESIRED  STEP  SIZE  DESIRED  STEP  SIZE  DESIRED  STEP  S 


STEP  INPUT  EXAMPLE 

DSL  PROGRAM—  R0BSIM2 

FINTIME  =  0.015 

Figure  4.1  (a)-(e) 

i 

1 

1 

1 

l 

FINTIME 

1 

1 

S'  RAMP  EXCITATION 

S^  FINTIME  =  0.015 

TV1  =  FINTIME /2 
yS  FIGURE  4.2  (a)-(e) 

1 

1 

1 

1 

FINTIME 

RAMP  EXCITATION 

1 

■ 

FINTIME  =  0.015 

1 

Figure  4.  3(a)-(e) 
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1 

1 

=  FINTIME*(2/3) 
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FINTIME 


STEP  EQUATION  IS:  (DESIRED  STEP  SIZEXSTEP(O.O)) 

RAMP  EQUATION  IS  :  (DESIRED  STEP  SIZEXTIME  -RAMP(TVI)) 

(TV  1 ) 


Figure  4.4  Excitation  Examples  of  STEP  and  RAMP  Excitation 


Figure  4.5  Modifications  to  R0BSIM2  for  Multiple  Runs 
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ROBSIM  1 

CONTRL 

FINTIME 

0.06 

DELT 

0.00005 

DELS 

0.00025 

SAVE  AT  0.0004 

ROBSIM  2  AND  2N 

CONTRL 

FINTIME 

0.015 

DELT 

0.00001 

DELS 

0.00005 

SAVE  AT  0.00008 

ROBSIM  3 

CONTRL 

FINTIME 

0.005 

DELT 

0.0000025 

DELS 

0.00001 

SAVE  AT  0.00002 

The  only  difference  in  the  three  simulation  programs  are 
the  CONTROL  parameters  to  the  DSL  program  and  how  the 
graphs  were  plotted  on  the  TEK618.  These  programs  were 
used  to  decide  on  the  values  that  would  best  suit  the 
multiple  runs  that  are  to  be  done. 

Figure  4  6  Modifications  to  ROBSIM1 , 2,  2n,  and  3 


A.  INTRODUCTION 

Several  techniques  have  been  developed  to  teach  robots  to  perform  the 
desired  tasks  and  movements.  The  most  significant  of  these  are  1 .)  Manual 
teaching,  2.)  Lead-through  teaching,  and  3.)  Programming.  All  of  these 
methods  require  the  operator  to  manually  lead  the  robot  through  the  desired 
task  and  then  playback  the  resulting  stored  data  or  to  develop  sophisticated 
task  level  programs  that  interface  with  the  real  world  using  position  and 
velocity  sensors.  As  stated  earlier  the  intent  of  this  thesis  is  to  develop  a 
method  of  predetermining  the  control  signals  for  each  of  the  robot  arms 
with  an  accuracy  that  eliminates  the  need  for  elaborate  sensing  equipment. 
Using  a  highly  accurate  positioning  servo  control,  signals  can  be  applied 
based  on  position  alone  which  allows  the  operator  to  precalculate  the  entire 
path  that  the  robot  arm  is  to  follow. 

B.  MANUAL  TEACHING  and  LEAD-THROUGH  TEACHING 

Sometimes  called  teaching-by-showing  or  guiding  involves  manipulating 

the  robot  arms  through  the  desired  patterns,  storing  the  resulting  time  and 

position  coordinates  in  memory,  and  playing  back  the  coordinates  in  the 

desired  sequence.  The  Manual  Teaching  method  does  not  require  a  computer 

to  control  the  movements  and  is  used  mainly  in  repetitive  tasks  such  as 

welding,  painting,  or  material  handling.  The  Lead-Through  requires  a 

computer  or  simulator  to  control.  Disadvantages  to  both  methods  include 
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large  memory  requirements,  unintentional  motions  will  be  recorded  as  the 
arm  is  manually  lead  through  the  task,  high  precision  is  not  possible,  and 
the  exact  velocity  of  the  arm  cannot  be  controlled. 

C.  PROGRAMMING  TECHNIQUES 

Programming  of  complex  or  random  maneuvers  allows  the  robot  arm  to 
be  accurately  positioned.  Robot  level  programming  correlates  the  robots 
sensor  data  and  then  specifies  the  desired  movement.  Motion  by  the  robots 
arm  can  be  predetermined  by  either  of  the  teach  methods  described  earlier 
and  uses  sensors  to  tell  the  position  of  an  object  and  bases  the  motion  of 
the  robot  relative  to  the  coordinate  system  of  the  object.  With  Task  level 
programming  the  user  describes  the  task  and  the  task-planner  formulates  a 
robot  level  program  to  carry  out  that  task.  Either  of  the  level  programming 
techniques  require  a  special  language  to  be  used  in  controlling  the  robot. 

D.  COMPUTER  PATH  DEVELOPMENT 

Instead  of  developing  the  coordinates  for  the  robot  movement  by  the 
teach  methods  we  are  precalculating  the  required  robot  control  signals  and 
then  applying  these  control  signals  to  each  of  the  arms  in  an  independent  but 
simultaneous  manner.  First  the  desired  path  that  the  robot  is  to  follow  is 
described  and  then  these  values  are  adjusted  for  the  type  of  robot  arm 
positioning  syncros  being  used  that  were  described  in  Chapter  Three. 

Finally,  in  Chapter  Six,  the  desired  control  signals  are  applied  to  the 
simulated  model  and  the  motion  of  the  simulator  is  compared  to  the  desired 


Options  in  picking  the  desired  path  have  been  limited  to  make  the 
program  manageable.  The  following  is  a  list  of  the  assumptions  and 
restrictions  in  picking  the  path: 

1 .  Paths  are  of  the  form  Y  =  f(X)  and  Z  =  f(X,Y). 

2.  Start  and  finish  points  are  inputted  as  two-tuples  of  (X,Y).  Start  and 
finish  points  in  the  Z-direction  are  calculated  from  the  equations  that 
are  implemented  in  the  program.  Motion  in  just  one  direction  can  not 
be  accomplished  in  the  cartesian  coordinate  system  because  of  the 
difficulty  in  describing  the  motion.  A  parametric  routine,  added  to 
make  conic  section  equations  easier  to  program,  will  be  able  to  easily 
handle  these  types  of  equations. 

3.  All  values  must  result  in  positive  values  that  will  be  weighted  as 
desired  by  the  user.  This  weight  factor  is  a  linearization  so  that  the 
path  coordinates  are  within  the  range  of  the  robot.  This  is  termed 
scaling  and  will  be  discussed  further  in  Chapter  Six. 

4.  Maximum  travel  lengths  are  based  on  the  maximum  velocity  of  the 
separate  arms  and  by  the  differences  in  each  of  the  arm  torques.  The 
current  programs  assume  that  the  velocities  are  equal  to  the 
maximum  step  length  divided  by  a  fixed  delta  T  and  are  scaled  so  that 
the  maximum  step  sizes  are  all  equal  in  all  three  coordinate 
directions. 

5.  The  user  has  a  choice  of  going  home  after  each  movement  or  going  to 

the  start  point  of  the  next  equation.  If  the  start  of  the  next  equation 

is  the  same  as  the  finish  of  the  previous  you  must  do  a  FSTAR 
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subroutine  to  align  the  index  variables.  This  inserts  a  repeated  step 
where  the  robot  goes  nowhere  and  does  not  increase  delay  time 
significantly.  The  cost  to  the  DSL  program  is  just  a  few  instructions 
before  the  program  goes  and  gets  the  next  point. 

6.  Three  equation  capability  was  chosen  for  path  description  in  this 
system  for  simplicity,  yet  being  sufficiently  long  enough  to 
demonstrate  the  required  path  structure  (ROBPATH1).  A  two  equation 
capability  will  be  discussed  as  the  primary  program  later  in  this 
chapter  (ROBPATH4). 

7.  The  program  has  been  limited  to  1000  points  of  (X.Y.Z)  coordinate 
points  and  a  maximum  distance  for  the  each  movement  has  been 
chosen  to  be  0.10  units  to  allow  each  of  the  points  to  be  discernible. 

To  observe  smoothness  a  maximum  distance  for  movement  will  be 
reduced  to  0.01  or  0.001  depending  on  the  application.  These  will  be 
demonstrated  further  in  Chapter  Six. 

8.  Movement  in  a  plane  parallel  to  the  axis  has  been  accounted  for. 

9.  Due  to  the  method  of  scaling  the  start  and  finish  points  of  each  arm 
must  be  different  and  distinct  relative  to  the  X  coordinate  axis.  That 
is  the  start  and  finish  points  must  be  in  the  direction  of  the  slope  of 
the  desired  curve.  This  determines  the  direction  that  the  X  increment 
is  taking  so  that  when  applied  to  y=f(x)  we  get  a  legitimate  y 
increment. 
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Since  the  robot  of  concern  here  is  a  cartesian  robot  the  allowable 
motion  has  been  restricted  to  the  first  octant  where  all  of  the  (X,Y,Z) 
coordinate  points  are  positive  and  the  coordinate  system  is  right-handed. 

This  means  that  the  program  will  not  be  sufficient  if  used  with 
non-cartesian  robots  without  some  modifications  or  transformations. 
Transformations  will  be  discussed  further  in  Chapter  Seven.  Figure  5.1 
gives  a  description  of  the  areas  that  are  required  to  be  modified  for 
successful  run  of  the  program.  Figure  5.2  flowcharts  the  input/change 
procedure  required  to  make  the  program  function.  The  full  program  listing 
in  both  FORTRAN  [WF77]  and  Microsoft  BASIC  are  located  in  Appendix  E  and 
F.  Figures  5.3  to  5.7  flowchart  the  program  main  and  the  four  subroutines 
used  in  the  ROBPATH4. FORTRAN  program.  Flowcharts  for  the  BASIC 
program  are  given  in  Appendix  F.  From  now  on  we  will  refer  to  the 
ROBPATH4. FORTRAN  listing  in  all  explanations. 

To  set  up  the  program  for  defining  the  desired  path,  or  series  of 
sub-paths,  of  the  robot  tip  (sometimes  called  the  endpoint)  follow  the 
procedure  set  up  in  Figure  5.2  and  as  seen  in  example  on  Figure  5.1 .  The 
user  must  first  decide  on  the  motion  that  he  wants  the  robot  tip  to  make 
following  the  restrictions  stated  earlier.  Once  the  path  has  been  chosen 
then  the  user  can  modify  the  FORTRAN  program  so  that  the  desired 
calculations  will  follow  the  path.  There  are  four  main  subroutines  which 
help  in  formulating  the  desired  path  movement.  They  are: 

1 .  HSTAR:  Describes  motion  of  the  Robot  Arm  from  Home  to  the  Start  of 

the  next  desired  path.  Motion  is  linear  between  Home  and  Start  points. 
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2.  STARF:  Describes  the  motion  of  a  path  from  the  Start  to  Finish  of 
the  desired  sub-path.  This  is  the  program  that  calculates  a  set  of 
three  tuples  of  position  points  based  on  user  inputted  equations  in 
Cartesian  Coordinates. 

3.  FSTAR:  Describes  the  motion  of  a  path  from  the  Finish  of  one 
sub-path  to  the  Start  of  the  next  sub-path.  Motion  is  linear  in  this 
subroutine. 

4.  FHOME:  Describes  the  motion  of  a  path  from  the  Finish  of  a  sub-path 
to  Home.  Motion  is  linear  in  this  subroutine. 

5.  SFPAR:  Describes  the  motion  of  a  path  from  Start  to  Finish  of  a  set 
of  X,  Y,  and  Z  parametric  equations  which  are  a  function  of  a  single 
variable.  The  variable  can  represent  radian  movement,  time 
movement,  or  can  represent  a  user  defined  variable.  Start  and  Finish 
points  are  user  supplied,  positive,  with  the  start  point  [TS]  having  a 
smaller  magnitude  then  the  finish  [TF].  User  must  take  this  into 
consideration  when  writing  the  parametric  equations. 

All  of  the  subroutines  are  similar  in  construction  except  STARF  and 

SFPAR.  They  assume  that  the  path  between  the  start  and  finish  points  are 

linear  and  use  these  points  to  scale  the  all  of  the  incremental  values  to 

within  the  maximum  distance  that  each  coordinate  can  move.  With  the 

subroutine  STARF  the  main  assumption  is  that  changes  in  X  [DELX] 

determine  the  value  of  the  change  in  Y  [DELY]  and  that  these  in  turn 

determine  the  incremental  change  in  Z  [DELZ]  First  the  value  of  the 

difference  in  the  X  start  and  finish  directions  are  used  to  determine  the 

direction  of  the  x  increment.  Then  this  value  is  applied  to  the  first 
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equation  [EQAT1]  to  determine  the  corresponding  y  increment.  If  this 
increment  is  greater  than  the  maximum  value  allowed  in  this  direction 
[MAXODY]  then  the  x  increment  is  linearly  scaled  using  the  following 
relationship: 

DELX  =  DELX  (MAXODY/ABS(DELY)).  [eqn  5 

This  scaling  is  repeated  until  both  values  are  within  their  prescribed 
maximum  values.  Then  the  x  and  y  increments  are  used  to  calculate  the  z 
increment  [DELZ]  using  the  second  equation  [EQUAT1].  If  the  z  increment  is 
larger  than  the  maximum  allowed  z  increment  [MAXODZ]  then  all  three 
increments  are  scaled  to  make  the  z  increment  equal  to  MAXODZ.  Then  all 
three  increments  are  added  to  the  previously  calculated  three  tuple  of 
coordinate  positions  [HSPOS  array]  and  the  process  is  repeated  for  the  next 
set  of  points.  Each  time  a  point  is  generated  there  is  a  check  to  see  if  the 
path  (or  sub-path)  has  reached  the  FINISH  point.  If  the  FINISH  point  has 
been  reached  then  the  last  position  value  is  set  equal  to  the  FINISH  point 
and  the  program  continues  on  to  the  next  subroutine.  The  program  also 
checks  for  plane  movement  and  if  plane  movement  is  noted  then  the  value 
of  the  previous  data  point  is  retained. 

As  each  of  the  subroutines  are  called  the  HSPOS  array  maintaining 
the  data  is  read  into  the  program  and  then  rewritten.  These  sets  of  data 
points  are  to  be  used  in  driving  the  DSL  Simulation  program  [chosen  in 
Chapter  Four]  and  to  compare  the  final  results.  The  main  array  is  HSPOS 
which  is  composed  of  a  the  total  number  of  three  tuples  in  the  array 


followed  by  columns  containing  the  three  tuple  of  coordinate  positions 
which  will  then  be  used  by  one  of  the  DSL  simulation  programs. 

Chapter  Six  takes  the  results  of  the  FORTRAN  program,  which  are  the 
desired  path  coordinates,  and  compares  each  point  with  the  results  out  of 
the  DSL  program.  It  will  also  contain  any  modifications  to  the  programs  if 
they  are  needed,  such  as  scaling. 


USER  MODIFICATION  REGION 
FUNCTION  MODIFICATION 

EQUATION  1-4  (EQAT  1 -4 /EQUAT 1 -4) 
PARAMETRIC  X/Y/Z  EQUATION  1-4 
MAIN  PROGRAM 
DELT 
MAXOD 

ACCURACY  (IF  USED) (ACC) 

MAX  VELOCITY  CONSTANTS  (CURRENTLY  1) 
DELT  (AGAIN) 

VALUE  MAXIMUM  (VALMAX) 

VALUE  MIINIMUM  (VALMIN) 

EQUATIONS  BOUNDS  AREA 

START  AND  FINISH  POINTS 
OUTPUT  S/F  POINTS 
PATH  MOVEMENT  REGION 

HOME  (DIFFERENT  FOR  DIFFERENT  ARM 
CONFIGURATIONS  AND  TYPE) 

PATH  FORMATION 
PRINTOUT  REGION 

ROBOT  SIMULATION  NUMBER 


CHECKING  THESE  PARAMETERS  ALONG  WITH  FIGURE  5.2 
WILL  GIVE  YOU  THE  PATH.  AFTER  UPDATING  THE 
FILDEFS  NEEDED  AND  COMPILING  IT  IS  RECOMMENDED 
THAT  A  THREE  DIMENSION  PLOT  BE  DONE  TO  INSURE 
THAT  THE  PATH  IS  AS  DESIRED. 


Figure  5. 1  Path  Program  Modification  Regions. 


Figure  5.2  Procedure  for  Updating  Path  Equat 
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Figure  5.3  Robot  Path  Formation  Flowchart.  ROBPATH4.FORTRAN 
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Figure  5,4  (Cont)  Subroutine  HSTAR  Home  to  Start. 
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Figure  5.6  Subroutine  FSTAR.  Finish  to  Start. 
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Figure  5.6  (Cont)  Subrouti 
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Figure  5.7  (Cont)  Subroutine  FHOME.  Finish  to  Home. 
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Figure  5,8  Subroutine  SFPAR  .  Parametric  Start  to  Finish. 
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Figure  5.8  (Cont)  Subroutine  SFPAR.  Parametric  Start  to  Finish. 
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A.  INTRODUCTION 


Interfacing  the  path  program  developed  in  Chapter  Five  with  the  DSL 
program  selected  in  Chapter  Four  has  resulted  in  some  interesting 
observations.  Since  the  overall  objective  of  the  thesis  is  to  get  a  cartesian 
robot  arm  to  follow  a  path  that  was  precalculated  it  would  seem  that  all 
that  needs  to  be  done  is  to  generate  a  set  of  coordinates  and  sequentially 
feed  these  values  into  the  robot  arm.  However  the  DSL  program,  which  is 
chosen  to  simulate  the  robot  arm  has  certain  control  parameters  that  work 
within  the  bounds  of  DSL  and  do  not  give  an  indication  of  the  type  of 
hardware  necessary  to  accomplish  the  task.  For  example,  DSL  has  several 
options  for  the  way  it  accepts  inputted  data  and  applies  this  data  to  the 
robot  arm.  Two  of  these  were  discussed  in  Chapter  Four  but  never  explained. 
If  the  robot  were  to  receive  a  set  of  coordinates  it  is  fairly  easy  to  see  how 
to  make  the  STEP  input,  but  not  readily  apparent  on  how  to  take  the  set  of 
coordinates  and  get  a  RAMP.  Subdividing  the  coordinates  in  a  linear  fashion 
and  inputting  these  at  a  constant  rate  is  one  answer,  but  that  is  the  same  as 
defining  smaller  increments  in  the  path  data  and  is  less  accurate. 

Another  problem  occurs  when  using  position  as  a  means  of  deciding 
whether  or  not  you  have  reached  the  end  of  the  step  and  are  ready  to  input 
the  next  set  of  data.  Using  position  can  be  accurate  at  the  points  that  were 
desired  in  the  path  equation  but  you  do  not  really  have  sufficient  control 
over  the  path.  It  is  probably  better  to  set  a  designated  time  interval  for 


each  path  length  and  use  this  to  control  the  rate  at  which  the  robot  arm  gets 
the  next  set  of  coordinate  points.  This  was  also  discussed  in  Chapter  Four. 

B.  SCALING 

The  path  generating  program  also  has  made  no  mention  of  the  length  of 
the  robot  arms.  Each  robot  arm  can  be  of  different  length  and  blindly 
assuming  that  the  robot  can  follow  all  of  the  points  without  any  problem 
may  lead  to  serious  problems.  The  DSL  program  also  assumes  that  the 
lengths  of  the  arms  are  sufficient.  Some  prescaling  may  be  needed  to  make 
all  of  the  movements  within  the  range  of  the  arm.  This  would  take  the 
maximum  value  of  the  coordinate  listing  and  scale  all  of  the  coordinate 
points  with  the  relative  length  of  the  arm  in  that  direction.  This  would  lead 
to  a  distortion  of  the  equations  used  to  generate  the  input.  An  alternative 
method  would  be  to  scale  everything  with  reference  from  the  largest  value 
to  a  unit  length,  causing  no  distortion,  and  then  setting  all  of  the  values  on 
the  smaller  robot  arms  to  a  max  value  while  that  position  is  being 
attempted.  It  would  be  similar  to  running  the  arm  up  against  a  stop  point 
and  maintaining  that  value  for  all  of  the  points  that  occur  outside  of  that 
region. 

It  is  the  latter  method  that  will  be  chosen  in  this  thesis.  After  the 
Path  generation  program  has  been  completed  a  path  boundary  program  will 
take  the  maximum  value  of  the  input  coordinate  points  and  scale  the  array 
so  that  this  value  is  one.  For  now  we  will  assume  that  all  arms  are  of  equal 
length  and  have  a  value  of  one.  Of  course  it  may  be  that  a  user  would  want  a 
larger  scale  value  and  with  a  minor  modification  that  can  be  entered. 
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C.  TEST  RUNS 

There  are  several  types  of  paths  that  need  to  be  considered  and  tested 
to  insure  proper  operation.  They  are: 

1 .  Linear 

2.  Large  curvature 

3.  Small  curvature 

4.  Repeated  motions 

The  curvature  paths  can  be  observed  by  using  conic  sections,  which 
include  mainly  circles,  ellipses,  parabolas,  and  hyperbolas,  or 
transcendental  functions,  which  are  mainly  composed  of  sine,  cosine,  or 
tangential  functions.  Since  it  would  be  difficult  to  test  all  of  the 
different  types  of  curves  we  have  arbitrarily  chosen  a  circular  function 
and  a  sine  function  as  test  platforms.  For  a  repeated  motion  the  right 
circular  helix  has  been  chosen.  These  movements  will  also  insure  proper 
operation  as  a  curve  moves  tangent  to  a  coordinate  plane.  The  final  motions 
will  be  that  of  a  straight  line  in  all  of  the  coordinate  axes  and  one  parallel 
to  each  coordinate  axis.  The  goal  is  to  demonstrate  flexibility  of  the 
program  and  find  problem  areas. 

Table  6.1  summarize  the  test  runs  that  that  will  be  studied.  Each  of 
these  runs  will  be  applied  to  both  the  STEP  and  RAMP  excitations  and 
Position  and  Velocity  Methods  discussed  in  Chapter  4.  Figure  6.1 
summarizes  the  equations  used  and  the  modifications  to  the  PATH  program. 


TABLE  6.1 
TEST  RUNS 


TEST  RUN 

EXCITATION 

METHOD 

MOTION 

1(a) 

STEP 

POSITION 

Linear  motion 

1(b) 

STEP 

VELOCITY 

1(c) 

RAMP 

POSITION 

Kd) 

RAMP 

VELOCITY 

2(a) 

STEP 

POSITION 

Circular  motion 

2(b) 

STEP 

VELOCITY 

2(c) 

RAMP 

POSITION 

2(d) 

RAMP 

VELOCITY 

3(a) 

STEP 

POSITION 

Helical  Motion 

3(b) 

STEP 

VELOCITY 

3(c) 

RAMP 

POSITION 

3(d) 

RAMP 

VELOCITY 

4(a) 

STEP 

POSITION 

Sinusoidal 

4(b) 

STEP 

VELOCITY 

4(c) 

RAMP 

POSITION 

4(d) 

RAMP 

VELOCITY 

D.  RESULTS 

i.  Linear. MotiQQ 

Linear  motion  is  one  of  the  more  basic  movements  and  also  one  of 
the  hardest  to  describe.  For  example  a  linear  movement  parallel  to  any 
axis  is  hard  to  put  to  an  equation  while  in  the  cartesian  coordinate  system 
if  not  using  parametric  equations.  Two  linear  motions  were  chosen  to 
show  that  the  program  works  correctly  in  both  the  cartesian  and  the 
parametric  equation  form.  They  are  described  by: 
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Linear  path  1 : 


Equation  1  : 

(1.0,0.25,0.25) 

to 

(0.25,1.0,1.0) 

Equation  2 : 

(0.5,1 .0,1.0) 

to 

(1.0, 0.5, 1.0) 

Linear  path  2: 

Equation  1  : 

(1 .0,0.25,0.5) 

to 

(1.0, 0.75, .05) 

Equation  2 : 

(0.75, 1.0, .05) 

to 

(0.25,1.0,0.5) 

Equation  3 : 

(0.0,1 .0,0.0) 

to 

(0.0, 1.0, 1.0) 

Equation  4 : 

(0.75,0.25,0.25) 

to 

(0.25, .075,0.75) 

Linear  Path  1  was  calculated  on  an  early  path  generating  program  using  only 
cartesian  coordinates  while  Linear  Path  2  was  generated  on  the  final  path 
program  mixing  cartesian  and  parametric  equations.  Linear  Path  1  was 
re-run  on  the  final  path  program  to  insure  proper  operation.  Parametric 
equations  were  used  to  generate  the  paths  parallel  to  the  axis  while 
cartesian  equations  were  used  to  generate  diagonal  paths  (however 
parametric  equations  would  be  just  as  easy  or  easier  to  use).  Equations 
used  to  describe  Linear  Path  1  and  2  are  outlined  in  Figure  6.1 .  Results  of 
the  Linear  Path  1  and  2  are  displayed  in  Figures  6.2  (a)-(d)  and  6.3  (a)-(d), 
respectively,  and  show  a  3-dimensional  view  of  the  desired  path,  the  Robot 
arm  position  with  respect  to  time  for  each  degree  of  motion,  each  of  the 
axis  plane  projections,  and  the  error  between  the  desired  path  inputted  and 
the  final  arm  position  resulting  from  the  simulations  for  the  STEP 
excitation,  Position  method  of  the  conditions  shown  in  Table  6.1  (la).  The 
other  conditions  listed  in  Table  6.1  for  linear  motion  were  run  but  showed 
nothing  significant  that  will  not  be  discussed  in  more  detail  in  the  next 
section  on  circular  motion  and  are  located  in  Appendix  H.  Observing  the  time 
versus  distance  curves  shows  smooth  motion  for  the  size  of  step  inputted 
while  the  plane  projection  plots  show  that  the  path  appears  to  be  following 
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the  equations  accurately.  Path  error  generated  in  each  of  the  coordinate 
axes  show  that  each  of  the  arm  axes  are  within  the  desired  0.001  inches  of 
accuracy  [VALMIN  in  the  simulation  program].  The  error  developed  by  the 
path,  model,  and  simulation  will  be  discussed  later  in  this  chapter. 

2.  Circular  Motion 

Test  runs  2(a)-(d)  studied  the  effects  of  circular  motion.  First  a 
single  quadrant  (ROBPATH3. FORTRAN)  was  run  to  see  the  effects  on  the 
curvature.  Then  when  attempting  to  do  a  right  circular  helix 
(ROBPATH5. FORTRAN)  a  problem  was  noted  when  the  x  increment  was 
approaching  a  tangent  with  the  y  coordinate  axis  (at  x-0).  That  was  when 
the  decision  to  do  a  complete  circle  was  made  (ROBPATH4.FPRTRAN).  Being 
more  interesting  than  one  quadrant  most  of  the  testing  will  be  done  on  the 
complete  circular  motion. 

The  circular  path  generated  was  linear  from  home  to  start  of  the 
circular  motion  using  the  HSTAR  subroutine  with  a  maximum  step  size  of  0.1 
units.  Then  the  path  followed  the  equations: 


Equation  1  : 


y  -  ((.25  -  (x-.5)2)1/2  +  .5) 

[eqn  6.1] 

z  -  0.5 

[eqn  6.2] 

Equation  2: 

y  -  (-(.25  -  (x-.5)2)1/2  +  .5) 

[eqn  6.3] 

z-0.5 

[eqn  6.4] 

which  describe  circular  motion  in  the  XY  plane,  with  an  increment  maximum 

102 


step  size  of  0.001 .  The  change  in  maximum  step  size  was  implemented  to 
show  that  the  path  can  be  made  smooth.  This  technique  will  be  implemented 
only  for  the  position  method  techniques  because  implementing  this 
technique  for  the  velocity  method  would  require  significant  changes  in  the 
DSL  simulation  and  would  affect  the  outcome  of  the  simulation.  For  the 
velocity  method  a  maximum  step  size  of  0.01  units  was  used  for  all  of  the 
paths.  After  finishing  the  path  movement  a  linear  path  to  home  was 
completed  with  a  maximum  step  size  of  0.1  units. 

Figure  6.4  (a)  shows  time  versus  simulated  arm  position  for  each  of 
the  coordinate  axes  while  Figure  6.4  (b)  shows  the  plane  projections  for 
each  of  the  coordinate  axes.  To  do  this  motion  a  total  of  505  coordinate 
points  were  computed  and  inputted  to  the  simulation.  As  each  step  reached 
the  desired  accuracy,  stipulated  by  VALMIN  of  the  simulation  program 
[Position  Method],  the  next  three  tuple  of  position  points  were  inputted  and 
the  simulation  was  run  again.  The  curve  is  very  smooth  and  quite  accurate. 
Figure  6.4  (c)  plots  the  error  between  each  of  the  desired  inputted  points 
and  the  resulting  simulation  movement.  As  you  can  see  the  accuracy  at  the 
final  positions  are  well  within  the  accuracy  described  by  the  desired 
Position  Method  value  of  0.001  inches. 

Figures  6.5  (a)-(c)  represent  the  circle  path  using  the  RAMP 
excitation  along  with  the  Position  Method  of  simulation  control.  The  time 
response  curves  show  a  more  linear  response  especially  near  discontinuities 
where  radical  changes  in  slope  occur.  For  example  when  comparing  Figures 
6.4  (a)  and  6.5  (a),  near  the  peaks  and  valleys  of  the  time  response  curves, 

there  is  a  definite  rounding  to  the  STEP  Excitation  time  response  curves. 
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Also  notice  that  there  is  a  significant  reduction  in  time  when  using  the 
RAMP  Excitation  over  the  STEP  Excitation.  Path  error  is  nearly  the  same  for 
both  the  STEP  Excitation,  Figure  6.4  (c),  and  the  RAMP  Excitation,  Figure  6.5 
(c),  however  it  is  interesting  to  note  that  the  RAMP  Excitation  error 
decreases  as  a  function  of  time.  There  is  also  a  time  delay  at  the  start  of 
the  time  response  curves  which  is  due  to  the  first  step  of  the  RAMP  being 
zero  and  the  desired  path  being  zero  at  the  same  time.  This  phenomenon  will 
be  observed  in  some  of  the  latter  RAMP  excitation  curves  also. 

Figures  6.6  (a)-(l)  and  Figures  6.7  (a)-(k),  for  the  STEP  and  RAMP 
excitation,  respectively,  show  the  effects  of  inputting  the  "next"  position  at 
shorter  intervals  of  time.  This  method  is  called  the  Velocity  Method.  Due  to 
the  large  number  of  figures  required  only  the  XY  plane  projection  and  the 
path  errors  for  each  axis  will  be  shown.  The  test  runs  for  the  Velocity  and 
Position  methods  are  listed  in  Table  6.2.  Comparison  of  the  XY  plane 
projections  for  the  STEP  Excitation,  Velocity  Method  show  that  the  error  in 
the  paths  stay  to  within  0.001  inches  until  the  inputted  time  interval 
reaches  0.008  seconds  when  there  is  a  sharp  increase  in  the  path  error  in 
the  X  direction.  The  Y  and  Z  path  error  remain  constant  until  0.005  seconds 
when  they  start  rising  at  about  the  same  rate  as  the  X  error.  When  RAMP 
Excitation  was  applied  to  the  simulation  program  with  Velocity  Method, 
similar  results  were  experienced  except  that  the  error  curves  exceeded 
0.001  inches  with  a  0.001  secs  shorter  input  interval  in  all  three  axis. 

Table  6.2  contains  an  estimation  of  the  maximum  error  for  both  the  STEP 
and  RAMP  Excitation  and  the  results  are  plotted  in  Figures  6.8  (a)-(c). 

Figure  6.6(1)  was  used  to  verify  the  DSL  Program  save  function  to  insure 


I 


that  the  same  plot  was  produced  when  the  save  statement  was  larger  than 
the  velocity  step  increment.  For  example,  whether  the  save  statement  for  a 
VAIMIN  of  .001  is  saved  at  time  intervals  of  .005  or  .001 ,  we  arrive  at  the 
same  result. 

TABLE  6.2 

CIRCULAR  MOTION  TEST  RUNS/  VELOCITY  METHOD 


PATH  PROGRAM 
SIMULATION  PROGRAM 
DATA  FILE 

FIGURE  NO. 

6.4  (a)-(d) 

6.5  (a)-(c) 

3CITY  METHOD: 

PATH  PROGRAM 
SIMULATION  PROGRAM 
DATA  FILE 


INPUT 

STEP  EXC 

mm 

lElfil 

X _ } 

0.020 

(a) 

0.10 

0.015 

(b) 

0.10 

0.010 

(c) 

0.25 

0.008 

(d) 

1.40 

0.007 

(e) 

2.50 

0.006 

(f) 

4.90 

0.005 

(9) 

8.00 

0.004 

(h) 

15.0 

0.003 

(i) 

30.0 

0.002 

(j) 

80.0 

0.001 

(k) 

300.0 

o.oor 

0) 

300.0 

ROBPATH4.  FORTRAN 

ROBSIM2T.FO RTR AN  or  RS I M2T6. FORTRAN 
PATH4.DATA 


STEP 

RAMP 

:  ROBPATH4.FORTRAN 
:  ROBSIM2V. FORTRAN  or  RSIM2V6.FORTRAN 
:  PATH4.DATA 
PEAK  ERROR  [milli-inches] 


£^B|| 

iHii 

•  ^*§1 

rv* 

I 

Mi 

in 

yJOJ 


Differences  in  the  Position  and  Velocity  method  can  be  seen  by 
comparing  STEP  Excitation,  Position  Method  with  STEP  Excitation,  Velocity 
Method,  where  the  desired  position  is  inputted  at  0.008  seconds  so  that  the 
total  time  is  around  4.0  seconds.  The  X  path  error  is  larger  in  the  Velocity 
Method  while  the  Y  and  Z  path  error  is  less.  The  plane  projections  appear 
identical,  however  the  time  response  are  dissimilar  in  both  the  X  and  Y 
direction.  The  X  direction  is  more  rounded  through  the  circular  part  for  the 
Velocity  Method  [see  Appendix  H  for  the  Velocity  Method  Time  Response 
Figures,  VALMIN=  0.008]. 

3.  Helical  Motion 

With  the  current  path  formation  program  it  is  difficult  to  do 

repetitive  movements,  unless  all  of  the  dimensions  are  repetitive  in  nature. 

The  circle  discussed  above  was  repetitive  in  all  3  dimensions.  When  an 

attempt  is  made  to  form  even  the  simplest  right  circular  helix  it  is 

necessary  to  keep  track  of  the  number  of  revolutions  and  the  process 

becomes  more  difficult.  To  solve  this  problem  a  parametric  subroutine  was 

added  to  the  path  generating  program  that  allows  X,  Y,  and  Z  to  be  expressed 

as  a  function  of  a  single  variable,  such  as  time  or  displacement.  This 

reduces  the  number  of  equations  needed  to  do  a  right  circular  helix  to  three. 

The  parametric  equations  and  modifications  to  ROBPATH5  program  are  listed 

in  Figure  6-9.  Two  runs  were  made  for  a  three  and  six  revolution  right 

circular  helix  and  are  show  in  Figures  6.10  (a)-(d)  and  6.12  (a)-(d)  for  STEP 

Excitation  and  Position  Method,  and  Figures  6.11  (a)-(c)  and  6.13  (a)-(d)  for 

RAMP  Excitation,  Position  method.  Figure  6.1 4  and  6.15  (a)-(c)  show 
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different  STEP  Excitation,  Velocity  Method  Simulations.  To  arrive  at  the 
same  accuracy  as  the  Position  Method  it  is  necessary  to  have  Velocity 
Method  Input  Intervals  (VMM)  of  0.020  seconds  which  is  about  the  same  as 


(number  of  points)  x  (VMM)  =  Position  completion  time 

which  is  as  expected.  Figure  6.18  shows  the  VMM  versus  path  errors 
overlayed  onto  the  circular  path  Figures  6.8  (a)  and  (b). 

To  insure  proper  operation  of  the  parametric  equation  the  start  point 
for  the  independent  variable  must  be  smaller  than  the  finish  point.  In  these 
examples  the  independent  variable  t  was  chosen  to  be  in  radians  and  the 
equations  adjusted  as  needed  to  get  the  desired  output.  Figure  5-8  shows 
the  flowchart  of  the  subroutine  used.  The  three  revolution  right  circular 
helix  took  231  input  position  points  to  simulate  while  the  six  revolution 
right  circular  helix  took  420  input  positions.  Figures  6.17  (a)  and  (b) 
confirm  that  the  VMM  for  the  six  revolution  right  circular  helix  with  ramp 
excitation  would  be  between  0.020  and  0.015  seconds.  Calculating  directly 
indicates  that  a  value  of  0.0155  would  be  adequate  to  drive  the  Velocity 
Method  path  error  under  0.001  inches. 

4.  Sinusoidal  Motion 

Two  sinusoidal  equations  that  were  generated  are: 

Y  =  f(X)  -  ABS  (  SIN  (  X  ) )  [eqn  6.5] 

Z  =  f(X,Y)  =  ABS  (COS  (2*PI*X))  [eqn  6.6] 

The  STEP  excitation,  Position  method  results  are  shown  in  Figure  6.19 
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(a)-(d)  along  with  a  three  dimensional  diagram  of  the  motion,  while  the 
RAMP  Excitation,  Position  Method  results  are  shown  in  Figure  6.20  (a)-(c). 

The  plots  are  correct  but  are  not  of  much  use.  There  are  a  couple  of  points 
of  discontinuity  which  are  caused  by  interaction  between  the  large 
distances  between  steps  inputted  and  the  absolute  function. 

E  POSITION  ACCURACY 

When  comparing  STEP  or  RAMP  Excitations,  with  Position  Method 
simulation  and  varying  the  value  of  VALMIN,  which  is  the  error  difference 
between  the  desired  position  (RX,RY,  RZ)  and  the  motor  position  (Cl ,  C2,  C3), 
while  the  Path  Error  is  the  difference  between  the  desired  position 
(RX,RY,RZ)  and  the  simulated  position  (CX,  CY,  CZ).  There  is  a  limit  to  how 
accurate  the  simulation  can  get  by  just  changing  VALMIN.  Table  6.3  list  the 
error  for  each  of  the  axis  when  following  the  circular  path  and  the  three 
revolution  right  circular  helix  using  the  Position  Method.  Only  50  position 
steps  were  utilized  to  keep  computing  time  reasonable. 

Since  there  was  no  movement  in  the  Z  direction  for  the  circular  path 
there  is  no  Path  Error  in  that  direction.  This  was  the  reason  that  the  helical 
path  was  run  also.  In  most  cases  the  path  error  is  half  of  the  desired 
VALMIN.  The  only  disagreement  is  in  the  Y  direction  where  there  seems  to 
be  a  limit  where  the  Y  path  error  levels  off  regardless  of  the  value  of 
VALMIN.  This  is  around  0.080  milli-inches  for  the  Circular  Path  and  0.060 
milli-inches  for  the  Helical  Path.  Smaller  values  for  the  circular  path  and 
the  helical  path  indicate  a  lower  limit  in  the  simulation  program  to  arrive 
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at  a  desired  accuracy.  XY  Plane  Projection  and  X,  Y,  and  Z  Path  Errors  are  for 
the  Velocity  Method  are  located  in  Appendix  H. 


TABLE  6.3 

ACCURACY  USING  POSITION  METHOD 


VALMIN 

CIRCULAR  PATH  ERROR  [milli-in] 

HELIX  PATH 

ERROR  [milli-in] 

[milli-in] 

1 

Z 

FIG. 

X 

1 

z 

FIG. 

50.0 

— 

— 

— 

— 

25.0 

25.0 

1.5 

H-4(a) 

10.0 

5.0 

5.0 

0.0 

H-3(a) 

5.0 

5.0 

0.1 

H-4(b) 

5.0 

2.5 

2.5 

0.0 

H-3(b) 

2.5 

2.5 

0.12 

H-4(c) 

1.0 

0.5 

0.5 

0.0 

H-3(c) 

0.5 

0.5 

0.16 

H-4(d) 

0.5 

0.25 

.25 

0.0 

H-3(d) 

0.25 

0.3 

0.11 

H-4(e) 

0.1 

0.05 

0.08 

0.0 

H-3(e) 

0.05 

0.06 

0.05 

H-4(f) 

0.05 

0.025 

0.08 

0.0 

H-3(f) 

0.03 

0.06 

0.03 

H-4(g) 

0.01 

0.018 

0.09 

0.0 

H-3(g) 

— 

FAILED - 

0.005 

0.02 

0.08 

0.0 

H-3(h) 

0.001 

— 

FAILED  - 

Failed  runs  were  due  to  large  computation  times,  or  problems  with  the 
simulation  in  resolving  integrations.  Several  different  integration 
techniques  were  attempted,  along  with  various  DELT  and  DELS  values,  but 
the  runs  did  not  improve  or  complete  more  than  a  few  path  points  out  of  the 
fifty  in  the  path,  while  the  paths  that  were  completed  successfully  were 
rapidly  done. 

F.  ERROR 

There  are  several  areas  of  error  involved  when  trying  to  get  accurate 
movement  of  the  robot  arm.  They  include: 
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1 .  Motor  simulation  model  error  in  assuming  that  the  model  can  be 
reduced  to  a  second  order  system. 

2.  Robot  Kinematics  and  Dynamic  equation  assumptions. 

3.  Computer  roundoff  errors.  While  DSL  runs  in  double  precision  there  are 
still  minimum  accuracies  that  are  fairly  large  when  compared  to  the 
accuracies  that  are  needed  in  the  simulation.  All  programs  other  than 
DSL  are  single  precision  and  susceptible  to  large  truncation  and 
round-off  errors. 

4.  Program  design  errors.  The  largest  error  involved  in  the  program  is  in 
assuming  a  minimum  value  for  several  of  the  operations.  In  the 
Position  Method  the  value  VALMIN  (E)  is  chosen  to  be  a  given  value.  The 
difference  between  Cl  and  CX,  C2  and  CY,  and  C3  and  CZ  is  another 
error.  Ideally  these  values  will  be  the  same,  but  that  is  not  always  the 
case.  In  the  previous  section  the  fact  that  the  error  in  the  Y  direction 

can  be  larger  than  the  value  of  VALMIN  shows  that  CY  is  not  the  same 
as  C2  by  a  considerable  amount.  This  shows  a  discrepancy  in  the 
computer  simulation  model  and  in  the  way  that  Path  Error  was  defined. 

The  effect  of  these  errors  is  to  create  a  sphere  of  error  around  each  of 
the  position  points.  Not  necessarily  the  same  in  all  three  coordinate  axes,  it 
is  easier  to  visualize  a  sphere  following  the  desired  path.  As  the  Robot  arm 
follows  the  desired  path  around,  the  error  effectively  becomes  a  cylinder 
around  the  desired  path.  Added  to  the  non-linearity  of  the  Robot  Arm 
between  points  the  error  cylinder  can  become  quite  large. 


Path  Equations  for  Linear  Path  1 
EQUAT1  =  0.500 

EQAT1Y  =  1.25 -X 

EQUAT2  =  1.00 

EQAT2Y  =  1.50 -X 

Start  and  Finish  Points  For  Linear  Path  2 
XS 1  =  1.00  XS2  =  0.50 

XF 1  =  0.25  XF2  =  1.00 


Path  Equations  for  Linear  Path  2 
PEQX 1  =  0.75 

PEQY  1  =  0.5  +  0.25  T 

PEQZ  1  =  0.00 

PEQX  2  =  0.75- 0.5  T 

PEQY  2  =  0.75 

PEQZ  2  r  0.25 

PEQX  3  =  0.25 

PEQY  3  =  0.75 -0.5  T 

PEQZ  3  =  0.00 

PEQX  4  =  0.25 

PEQY  4  =  0.25  +  0.75  T 

PEQZ  4  =  1.00 

EQUAT5  =  X 
EQAT5Y  =  1.5 -X 

Start  and  Finish  Points  For  Linear  Path  2 
TS  =  0.00  XS5  =  0.50 

TF  =  1.00  XF5  =  1.00 


Excitation,  Position  Method. 

XY,  XZ,  and  YZ  Plane  Projection 


Figure  6.2(c)  Linear  Path  1,  STEP  Excitation,  Position  Method. 

X,  Y,  and  Z  Path  Error. 
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Circular  Path,  STEP  Excitation,  Velocity  M 
XY  Projection  and  X,  Y,  and  Z  Path  Error. 


Circular  Path,  STEP  Excitation,  Velocity  Method  (T=0.010). 
XY  Projection  and  X,  Y,  and  Z  Path  Error. 


Figure  6  6(e)  Circular  Path,  STEP  Excitation,  Velocity  Method.  IT =0.0071 
XY  Projection  and  X,  Y,  and  Z  Path  Error. 


Excitation,  Velocity  Me 
X,  Y,  and  Z  Path  Error. 


PATH  ERROR 
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XY  Projection 


Circular  Path,  RAMP  Excitation,  Velocity  Method.  [T=0.010], 
XY  Projection  and  X,  Y,  and  Z  Path  Error. 


Circular  Path,  RAMP  Excitation,  Velocity  t 
XY  Projection  and  X,  Y,  and  Z  Path  Error. 
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Figure  6  7(e)  Circular  Path,  RAMP  Excitation,  Velocity  Method.  (T=0.0( 
XY  Projection  and  X,  Y,  and  Z  Path  Error. 
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Circular  Path,  RAMP  Excitation 
XY  Projection  and  X,  Y,  and  Z 


Circular  Path,  RAMP  Excitation,  Velocity  h 
XY  Projection  and  X,  Y,  and  Z  Path  Error. 


Circular  Path,  RAMP  Excitation,  Velocity  (■ 
XY  Projection  and  X,  Y,  and  Z  Path  Error. 


PEAK  STEP  ERROR [milli-in] 


□ 

X-AXIS 

♦ 

Y-AXIS 

■ 

Z-AXIS 

!=  8 


TIME  [Velocity  Method  Input  Interval] 

NOTE:  LINES  BETWEEN 
CONSECUTIVE  POINTS 
INDICATE  ESTIMATE  OF 
VALUES  AND  SHOW 
CONTINUITY  AND  DO 
NOT  INDICATE  ACUAL 
YALUES. 


Figure  6.8  (b)  Velocity  Method  RAMP  Excitation  Error 
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Parametric  Equations 


PEQX  =  0.5  *  COS  (  T  )  +  0.5 

PEQY  =  0.5  *  SIN  (  T  )  +  0.5 

For  Helix  Path  1 

PEQZ  =  T /(  6  *  3.141529) 
For  Helix  Path  2 

PEQZ  =  T /(  12*  3.141529) 
For  Both  in  Equation  Bounds  Region 
TS  =  0.0 

DELT  =  0.1 

For  Helix  Path  1 

TF  =  6*  PI 

For  Helix  Path  2 

TF  =  12*PI 


Figure  6.9  Parametric  Equations  and  Path  For  Right  Circul 

Helix's  1  end  2 


Helix  Path  1 
Time  Respons 
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Figure  6.1 1(c)  Helix  Path  l,  RAMP  Excitation,  Position  Method. 

X,  Y,  and  2  Path  Error. 
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5. 1 3(c)  Helix  Path  2,  RAMP  Excitation,  Position  Method 
X,  Y,  and  Z  Path  Error. 
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x  Path  I,  RAMP  Excitation,  Velocity  Method.  IT =0.0 1 5). 
Projection  and  X,  Y,  and  Z  Path  Error. 


x  Path  1,  RAMP  Excitation,  Velocity  Method.  [T=0.010) 
Projection  and  X,  Y,  and  Z  Path  Error. 
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Figure  6.19(c)  Sinusoidal  Path,  STEP  Excitation,  Position  Method. 

X,  Y,  and  Z  Path  Error. 
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Figure  6.20(c)  Sinusoidal  Path,  RAMP  Excitation,  Position  Method. 

X,  Y,  and  Z  Path  Error. 
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A.  INTRODUCTION 


As  mentioned  in  Chapter  Two  the  second  robot  arm  model  that  is  being 
considered  is  the  articulated  (or  revolute  )  robot  model.  Using  procedures 
similar  to  that  of  the  Cartesian  Robot  Arm  model  the  lagrangian  equations 
were  solved,  the  computer  simulation  model  was  generated,  and  the  DSL 
program  was  designed  (Kalogiros,  1987).  Appendix  G  shows  the  entire  DSL 
program  generation  along  with  single  runs  that  were  used  to  verify  and  test 
the  DSL  program  and  equations.  The  purpose  of  this  chapter  is  to  tie  the 
path  generating  program,  written  in  cartesian  coordinates,  to  the  DSL 
program  inputs,  which  are  in  articulated  coordinates.  To  do  the  coordinate 
axis  reposition  an  intermediate  transformation  to  spherical  coordinates  was 
required.  This  also  allows  spherically  generated  path  data  to  be  converted 
directly  into  articulated  coordinate  path  data.  To  interpret  the  results  it  is 
necessary  to  reconvert  the  articulated  path  back  into  spherical  or  cartesian 
coordinates  to  be  plotted. 

B.  COORDINATE  TRANSFORMATION'S 

To  transform  the  set  of  three  tuples  of  cartesian  coordinate  points  to 
the  three  tuple  of  spherical  coordinate  points  is  fairly  simple  with  just  a 
few  points  to  keep  in  mind.  Figure  7-1  (a)  shows  the  range  of  motion  for  a 
Cartesian  Robot  that  was  used  in  Chapter  Five  while  Figure  7-1  (b)  shows 
the  range  of  motion  for  the  articulated  robot.  Each  of  the  figures  show  the 
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designed  home  (reference)  position  used  and  Figure  7-2  shows  the  regions 
superimposed  upon  each  other. 

The  equations  used  to  transform  the  data  points  from  cartesian  to 
spherical  coordinates  are: 

p  -  ((XP-.5)2  +(YP-.5)2+(ZP-.5)2)1/2  [eqn  7-1  ] 

0  =  arccos  [((b)2+(c)2-(a)2)/(2bc)]  [eqn  7-2] 

(p  *  arccos  [((d)2+(p)2-(f)2)/(2d  p  )]  [eqn  7-3] 

where 

p  is  the  radial  unit  length  from  (.5,. 5, .5)  to  the  point  being 
transformed  (XP,YP,ZP). 

0  is  the  angle  parallel  to  the  XY  plane  referenced  in  the  x 
direction  to  (XP,YP,ZP). 

cp  is  the  angle  of  p,  referenced  in  the  z  direction  through  the 
point  (.5, .5,. 5). 

a,b,c,d,  and  f  are  lengths  described  in  Figure  7.1  (b). 

Equation  7-1  is  the  three  dimensional  Pythagorean  principle,  and  equations 
7-2  and  7-3  relate  the  coordinates  by  use  of  the  Law  of  Cosines  [Appendix 
A], 

Once  in  spherical  coordinates  the  three  tuple  must  be  transformed  into 

something  useful  to  the  robot  arm.  Called  the  articulated  coordinate 

system,  it  consists  of  a  three  tuple  of  angles  referenced  to  a  spherical 

coordinate  p  maximum  of  .5  units.  Figure  7.3  shows  the  spherical  to 
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articulated  reference  system  whose  resulting  equations  are: 

CPI  =  0  [eqn  7-4 

CP2  =  n  -  cp  +  arccos  (2  p )  [eqn  7-5 

CP3  =  n  -  2  (arccos[2  p  ])  [eqn  7-6 

Several  assumptions  were  made  in  the  calculations: 

1 .  Both  of  the  articulated  robot  arms  are  of  the  same  length.  If  they  are 
not  then  the  program  will  have  to  be  adjusted  by  using  the  equations  of 
Figure  7.3. 

2.  The  robot  operates  in  the  'up*  elbow  mode.  CPI  and  CP3  are  the  same  in 
either  mode.  CP2  has  a  change  in  the  sign  of  the  arccos  term  [+  for  'up' 
elbow  and  -  for  'down'  elbowj.  Figure  7.3  also  demonstrates  the  'down' 
elbow  configuration,  however  all  of  the  programs  are  solely  'up'  elbow. 

'Up'  and  'down'  elbow  considerations  are  important  when  considering 
loading  and  obstacle  avoidance. 

3.  To  make  the  programming  simple  it  was  assumed  that  the  spherical 
coordinate  system  exists  within  a  1  unit  diameter  sphere  giving  a 
maximum  radius  of  .5  units  and  arm  lengths  of  .25  units.  The 
articulated  robot  coordinate  system  really  doesn't  consider  actual  arm 
lengths  in  the  movements,  however  calculations  from  spherical  to 
articulated  must  have  magnitudes  less  than  or  equal  to  1  for  proper 
operation  of  the  cosine  and  sine  functions.  Also  any  scaling  or 
limiting  done  to  the  cartesian  and  spherical  coordinate  systems  prior 
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to  converting  to  the  articulated  coordinate  system  must  be  undone  or 
at  least  considered  when  interpreting  the  results. 

To  interpret  the  motion  that  the  robot  is  making  it  is  necessary  to 
convert  the  articulated  coordinate  system  back  into  either  the  spherical  or 
cartesian  coordinate  system.  Figure  7.4  contains  a  listing  of  the  available 
coordinate  conversion  routines  as  well  as  a  listing  of  the  order  in  which  to 
program  them.  The  program  TRANSFOR. FORTRAN  when  set  up  correctly 
allows  transformation  from  spherical  to  articulated  and  then  back  to 
cartesian  and  articulated  to  spherical.  The  reason  for  transforming  the 
first  series  back  to  spherical  is  to  check  that  the  final  product  is  the  same 
as  the  original  if  no  scaling  or  limiting  is  done  and  it  also  gives  the  desired 
simulation  results  when  scaling  or  limiting  is  used  prior  to  the  simulation. 

C.  SCALING  AND  LIMITING  ROUTINES 

To  insure  that  the  motion  of  the  robot  is  within  the  range  of  possible 

movement  of  the  robot  arm  it  is  necessary  to  scale  or  limit  the  motion. 

Scaling  takes  the  largest  value  of  distance  and  normalizes  the  path  three 

tuples  to  one.  There  is  also  a  scaling  factor  [SF]  which  then  scales  the 

result  to  the  desired  values.  Scaling  is  available  for  the  X,  Y,  and  Z  values 

of  the  cartesian  coordinate  system  or  for  p  in  the  spherical  coordinate 

system.  It  is  important  to  remember  that  if  the  SF  is  between  0  and  1  then 

the  path  shrinks  and  the  maximum  value  of  the  desired  path  will  be  less 

than  or  equal  to  one  while  if  the  SF  is  greater  than  one  then  the  path  will  be 

greater  than  1 .  To  insure  proper  operation  when  converting  to  the 
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articulated  system  it  is  mandatory  that  the  spherical  system  be  scaled  (if 
desired)  and  limited. 

Limiting  takes  the  results  of  the  inputted  path  data,  finds  all  the  values 
greater  than  1  in  the  desired  cartesian  path  [.5  for  p  in  the  spherical]  and 
replaces  these  values  with  1 .  This  prevents  running  the  robot  arms  past 
their  capabilities  and  with  scaling  can  lead  to  some  interesting  paths.  Both 
scaling  and  limiting  routines  are  listed  in  Figure  7.4  and  Appendix  G. 

D.  TEST  RUN  RESULTS 

To  test  the  program  for  proper  operation  of  the  DSL  program  the  paths 

generated  for  the  cartesian  robot  were  converted  and  used  as  outlined  in 

Figure  7.5.  An  adjustment  was  made  to  the  DSL  program  which  inserted  the 

first  three  tuple  as  the  'home'  position  so  that  the  paths  would  not  have  to 

be  reconstructed  using  the  spherical  home  as  the  starting  point.  This 

reduces  the  errors  in  the  first  step  to  within  an  acceptable  amount  in  both 

time  and  distance  and  does  not  have  any  effect  if  you  start  from  'home'  [the 

first  position  should  be  home  in  any  case].  Table  7.1  show  the  paths  that 

were  used  and  the  Figures  associated.  When  viewing  the  three  dimensional 

plots  there  is  a  line  from  the  last  point  to  zero  that  is  not  resident  in  the 

cartesian  data,  either  after  conversion,  bypassing  simulation  (SCPATH  . 

DATA) ,  or  after  simulation  and  conversion  back  into  cartesian  coordinates 

(DCPATH  .  DATA),  that  is  unaccounted  for  and  is  an  error  in  the  DISSPLA 

System  program.  All  of  the  runs  are  in  the  STEP  Excitation,  Position  Method 

format.  Programs  for  the  RAMP  Excitation,  Position  Method  and  the  STEP 

and  RAMP  Excitations,  Velocity  Method  were  written  and  modifications  are 
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located  in  Appendix  D,  but  no  runs  were  done  to  check  for  proper  operation 
because  of  the  time  involved  in  even  the  shortest  run.  Twenty  to  thirty  CPU 
minutes  are  not  uncommon  when  calculating  a  500  point  path  with  the 
current  simulation  model. 


TABLE  7.1 

ARTICULATED  ROBOT  ARM  PATH  MOVEMENT 


PATH  PROGRAM 

TYPE  PATH 

FIGURES 

NUMBER  OF 
POINTS 

PATH2.data 

LINEAR  1 

7.5  (a)-(e) 

80 

PATH3.data 

LINEAR  2 

7.6  (a)-(e) 

139 

PATH4.data 

CIRCULAR 

7.7  (a)-(e) 

505 

PATH5.data 

HELICAL  1 

7.8  (a)-(e) 

23 1 

PATH5A.data 

HELICAL  2 

7.9  (a)-(e) 

420 

PATH6.data 

LINEAR 

7.10  (a)-(e) 

233 

PATH7.data 

SINUSOIDAL 

7.11  (a)-(e) 

345 

(a)  TIME  RESPONSE  (radians) 

(b)  PATH  ERROR  (milli-rads) 

(c)  ORIGINAL  PATH  IN  3-DIMENSIONS 

(d)  DIRECT  CONVERSION  PATH  IN  3-DIMENSIONS 

(e)  SIMULATED  CONVERSION  PATH  IN  3-DIMENSIONS 


As  with  the  Cartesian  Simulation  model  for  Position  Method  the  accuracy 
was  determined  by  E  [VALMIN]  dropping  below  the  value  of  0.001  inches. 
Path  Error  is  calculated  by  taking  the  difference  between  the  desired 
position  and  the  computer  simulation  model  output  and  E  is  calculated  by 
taking  the  difference  between  the  desired  position  and  the  motor 
simulation  model  output. 

During  the  testing  of  the  Articulated  Model  some  interesting  problems 

were  encountered.  Due  to  using  paths  designed  in  cartesian  coordinates,  the 
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articulated  coordinate  inputs  to  the  simulation  are  not  controlled  as  to  the 
size  of  radian  step  from  one  point  to  the  next.  When  far  from  the  center  of 
the  Articulated  Coordinate  System  there  is  not  any  problem  as  the  radian 
step  is  small  but  as  you  get  closer  to  the  center  of  the  coordinate  system  a 
small  step  in  Cartesian  Coordinates  result  in  a  large  step  in  Articulated 
Coordinates.  Consider,  for  example,  a  step  from  one  side  of  the  Articulated 
Coordinate  System  origin  directly  through  the  center  (0.5,  0.5,  0.5)  to  the 
other  side  of  the  origin  is  translated  into  a  180  degree  or  n  radian  step  in 
the  Articulated  Coordinate  System.  Associated  with  this  problem  is  that 
with  the  simulation  we  can  assume  an  infinitely  thin,  essentially 
nonexistent  arm,  where  in  reality  there  is  a  lot  of  space  where  the  arm  can 
not  go  due  to  support  structures.  With  this  in  mind  it  makes  no  sense  to 
worry  about  going  through  the  Articulated  origin  because  the  space  is 
already  occupied.  "Home"  has  also  been  described  as  (0.5,  0.5,  0.5)  and  any 
time  that  it  is  hit  from  whatever  direction  the  Arm  whips  around  to  "home" 
in  the  articulated  coordinate  system  (0.0,  3tt/2,0,0)  and  then  back  to  the 
next  position.  By  devising  a  scheme  to  prevent  the  arm  from  entering  a 
sphere  around  the  articulated  origin  most  of  the  problems  can  be  avoided. 
This  was  not  implemented  as  none  of  the  paths  went  close  to  the  origin. 

The  next  problem  came  about  in  the  definition  of  CPI ,  which  is  the  first 

axis  in  the  Articulated  Coordinate  System.  Measured  in  radians  the  arm 

travels  from  C.O  to  2n  radians  without  any  difficult.  When  going  counter 

clockwise  from  a  number  near  2tt  to  a  number  just  above  zero  a  problem 

occurs.  With  the  type  of  control  we  are  using  the  Arm  would  try  to  i  3spond 

by  going  clockwise  back  around  the  axis  center  until  it  arrives  at  the  new 
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number.  This  is  undesirable  and  occurs  in  most  of  the  paths  that  were 
generated.  To  solve  the  problem  a  comparison  of  the  next  position  with  the 
old  position  is  done.  If  it  meets  the  criteria  described  above  then  the  old 
position  is  subtracted  from  2n  and  renamed  the  old  position.  This  is  easy 
to  implement  in  software  but  in  hardware  may  be  difficult. 

All  of  the  Path  errors  are  within  the  desired  VALMIN.  When  looking  at 
some  of  the  Path  Error  versus  time  remember  that  2tt  radians  and  zero  are 
the  same  accounting  for  some  of  the  seemingly  large  errors.  This  problem 
could  have  been  remedied  by  taking  the  2n  correction  mentioned  earlier  and 
applied  it  to  both  the  motor  output  (Cl)  and  the  computer  simulation  output 
(CX)  but  was  actually  only  applied  to  the  motor  output  (Cl).  The  overall 
results  are  accurate  and  within  the  desired  specifications. 


v'-  v-  w.  w.  w._  s.’'-:''- 


Figure  7.1  (a):  Cartesian  Range  of  Motion 
(b)  :  Spherical/  Articulated  Range  of  Motion 
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Cartesian  to  Spherical  Coordinate  Transformation 

p  =  ((XP-.5)2*  (YP-.5)2  +  (ZP-.5)2)1/2 

9  =  arccos  [((b)2  +  (c)2- (a)2)/  (2bc)] 
<p  =  arccos  [((d)2+ (p)2- (f)2)/ (2dp)] 


Choose 

a  =  «XP  -  I)2  +  (YP  -  ,5)2)1/2 
b  =  ((XP  -  .5)2  +  (YP  -  ,5)2)1/2 

c  =  .5 

d  =  .5 

f  =  ((XP  -  ,5)2  +  (YP  -  ,5)2  +  (ZP  -  1)2)1/2 

Where  (  XP,  YP,  ZP  )  is  the  point  being  converted  from 
Cartesian  to  spherical. 


Figure  7.2  Cartesian  to  Spherical  Coordinate  Conversion. 


Given  (p,6,<p)  as  Spherical  Coordinates 
to  find  (CPI,  CP2,  CP3)  in  Articulated  Coordinates. 

CPI  =0 

CP2  =  180-<p  +  <x3  (elbow ’up') 

CP3  =  arccos  [((d2)2  +  (d3)2  -  (p)2)  /  (2  (d2)(d3))  ] 


Since  d2  =  d3  =  .25  or  max  reach  of  p=  .5 
then  <x2  =  a3  and 
a  1  =  180  -  2(a2)  =  CP3 

=  arccos  [  ( .5  -  (p) 2  )] 


PROGRAM  (FORTRAN) 

PURPOSE 

R0BPATH2 

GENERATES  LINEAR  PATH  1 

R0BPATH3 

GENERATES  LINEAR  PATH  2 

ROBPATH4 

GENERATES  CIRCULAR  PATH 

R0BPATH5 

GENERATES  RIGHT  CIRCULAR  HELIX  1 

R0BPATH5A 

GENERATES  RIGHT  CIRCULAR  HELIX  2 

R0BPATH6 

GENERATES  LINEAR  PATH 

R0BPATH7 

GENERATES  SINUSOIDAL  PATH 

TRANSFOR 

CARSP 

CARTESIAN  TO  SPHERICAL 

CARLI 

CARTESIAN  LIMITING 

CARSC 

CARTESIAN  SCALING 

SPHLI 

SPHERICAL  LIMITING 

SPHSC 

SPHERICAL  SCALING 

j  SPART 

SPHERICAL  TO  ARTICULATED 

ARTSP 

ARTICULATED  TO  SPHERICAL 

SPCAR 

SPERICALTO  CARTESIAN 

RSIMREV 

ARTICULATED  ROBOT  MODEL 

RSIMREV 1 

SINGLE  STEP  INPUT 

ARTICULATED  ROBOT  MODEL 

ROBCOMP 1 

MULTIPLE  STEP  INPUTS 

3-DIMENSIONAL  PLOTTING  ROUTINE 

Figure  7.4  Articulated  Robot  path,  transformation,  scaling,  and 
limiting  programs  and  subroutines 
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TinE  RESPONSE 


3D  PRTH  TRRJECTORY 

ROBOT  PATH  MOTIONS 


3D  PATH  TRAJECTORY 

ROBOT  PATH  MOTIONS 


Figure  7.6(d)  Articulated  Robot  Linear  Path  I  Direct  Conversion  Path  in 
3-Dimensions.  [SCPATH2.DATA],  No  Simulation. 
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3D  PATH  TRAJECTORY 

ROBOT  PRTH  MOTIONS 
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Figure  7.6(e)  Articulated  Robot  Linear  Path  I  Simulated  Conversion  Path  in 
3-Dimensions  [DCPATH2.DAT A).  Post  Simulation. 
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Figure  7.7(a)  Articulated  Robot  Linear  P8th  2  Time  Response 
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Figure  7.7(e)  Articulated  Robot  Linear  Path  2  Simulated  Conversion  Path  in 
3-Dimensions  [DCPATH3  DATA],  Post  Simulation. 
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Figure  7.8(c)  Articulated  Robot  Circular  Path  in  3-Dimensions 
(PATH4.DAT A).  Pre-simulation. 
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3D  PATH  TRAJECTORY 

ROBOT  PATH  MOTIONS 


Figure  7.8(d)  Articulated  Robot  Circular  Path  Direct  Conversion  Path  in 
3-Dimensions  (SCPATH4.DAT  A)  No  Simulation. 
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3D  PATH  TRAJECTORY 

ROBOT  PATH  MOTIONS 


3D  PfiTH  TRAJECTORY 

ROBOT  PfiTH  MOTIONS 


3-Dimensions.  (SCPATH5.DATA],  No  Simulation. 

203 


3D  PATH  TRAJECTORY 

ROBOT  PATH  MOTIONS 


rticulated  Robot  Helix  Path  2in. 
DATH5A.DATA],  Pre-simulation. 


30  PATH  TRflJECTORY 

ROBOT  PATH  MOTIONS 


Figure  7.10(d)  Articulated  Robot  Helix  Path  2  Direct  Conversion  Path  in 
3-Dimensions  1SCPATH5A  DATAj.  No  Simulation. 
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TIME  RESPONSE 


1(a)  Articulated  Robot  Linear  Path  Time  Response 
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3D  PATH  TRAJECTORY 

ROBOT  PATH  MOTIONS 


Figure  7  1 1(d)  Articulated  Robot  Linear  Path  Direct  Conversion  Path  in 
3-Dimensions  (SCPATH6  DATA)  No  Simulation 
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3D  PRTH  TRAJECTORY 

ROBOT  PATH  MOTIONS 


Figure  7  1 1(e)  Articulated  Robot  Linear  Path  Simulated  Conversion  Path  in 
3-Dimensions  IDCPATH6  DATA]  Post  Simulation. 


Figure  7.12(a)  Articulated  Robot  Sinusoidal  Path  Time  Response 
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Figure  7.12(c)  Articulated  Robot  Sinusoidal  Path  in  3-Dimensions. 
(PATH7.DATA],  Pre-simulation. 


3D  PATH  TRAJECTORY 

ROBOT  PATH  MOTIONS 


Figure  7.12(d)  Articulated  Robot  Sinusoidal  Path  Direct  Conversion  Path  in 
3-Dimensions.  (SCPATH7.DATA],  No  Simulation. 
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3D  PATH  TRAJECTORY 

ROBOT  PATH  MOTIONS 
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Figure  7.12(e)  Articulated  Robot  Sinusoidal  Path  Simulated  Conversion 
Path  in  3-Dimensions  (DCPATH7  DATA).  Post  Simulation. 
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In  this  thesis  we  have  shown  that  it  is  possible  to  have  a  Robot  follow  a 
desired  path  accurately.  Seven  paths  were  used  on  both  the  Cartesian  Robot 
and  the  Articulated  Robot  with  good  results,  giving  accuracies  on  the  order 
of  10-4. 

Each  of  the  paths  were  described  using  combinations  of  cartesian  and 

parametric  equations  and  were  linked  together  by  linear  subroutines.  The 

output  of  the  path  equations  is  a  table  of  cartesian  coordinates  which  are 

designed  so  that  the  maximum  movement  in  any  direction  is  controlled  by 

the  user  (MAXOD).  Also  under  the  users  control  is  VALMIN,  which  is  the 

error  between  the  computed  position  and  the  desired  position;  the  accuracy 

(ACC),  which  allows  higher  accuracy  of  movement  in  the  Position  Method; 

the  maximum  velocity  in  each  of  the  coordinate  axes  directions  (MAXVX, 

MAXVY,  and  MAXVZ);  and  the  change  in  time  increment  for  the  parametric 

routine.  The  user  also  decides  on  whether  to  plan  on  having  STEP  or  RAMP 

Excitation  to  the  Computer  Simulation  Model  and  the  method  for  which  the 

next  set  of  points  are  generated.  The  Position  Method  is  based  on  the 

accuracy  of  E  -  R  -  C  (Figure  3.2)  being  less  than  the  value  of  VALMIN. 

When  all  of  the  d.o.f.  meet  that  condition  the  the  next  position  is  inputted. 

The  Velocity  Method  ignores  accuracy  and  inputs  the  next  position  at  a  set 

time  interval.  The  advantage  of  the  Velocity  Method  is  that  you  can  get  the 

Robot  to  move  faster  with  only  a  slight  loss  in  position  accuracy  and  since 

one  of  the  leading  assumptions  made  was  that  the  maximum  error  in  any 
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step  is  less  than  the  distance  travelled  during  the  step  then  the  Velocity 
Method  is  a  good  choice  for  most  applications. 

The  Computer  Model  assumes  that  a  saturated  condition  may  be  reached 
during  the  generation  of  the  voltage  that  is  driving  ideal  joint  servo  but  the 
nonlinearities  introduced  do  not  significantly  affect  the  accuracy  of  the 
desired  movement  and  the  total  of  the  errors  stay  within  the  limit  of  the 
step  size. 

The  programs  are  based  on  the  movement  space  being  a  1  inch  cube  for 
the  Cartesian  Robot,  and  a  1  inch  diameter  sphere  for  the  Articulated  Robot. 
With  maximum  errors  of  10-4  inches  there  may  not  be  enough  accuracy  for 
some  applications.  For  example  applying  the  path  to  a  10  meter  cubed 
Cartesian  Robot  would  give  accuracies  on  the  order  of  1  mm.  For  several 
applications  this  would  not  be  sufficient  accuracy.  To  correct  this  problem 
it  might  be  a  better  approach  to  set  up  the  movement  space  of  the  Cartesian 
Robot  to  be  a  1000  unit  cube  with  step  sizes  being  1  unit.  The  problem  with 
this  is  that  there  may  be  excessive  amount  of  points  generated  and  that 
there  may  be  too  much  accuracy  for  the  desired  job  resulting  in  costly 
computation  times  that  are  not  needed.  As  a  result  the  size  and  application 
of  the  Robot  need  to  be  considered  prior  to  designing  the  path  generating 
program. 

A  lot  of  areas  need  to  be  researched  further.  Among  those  are: 

1.  The  affects  of  gravity  and  load  on  the  path  movement. 

2.  Perturbations  that  may  occur  due  to  shifting  loads  or  external  forces 
applied  to  the  robot. 

3.  Sensing  the  Robots  position  accurately. 


4.  Obstacle  avoidance  with  not  only  the  tip  of  the  Robot  but  ail  of  the 
joints  and  links. 

5.  Attachment  of  a  wrist  onto  the  Robot  Arm  and  the  algorithms  required 
to  move  the  arm  and  wrist  into  any  orientation  that  may  be  desired. 

6.  Generating  the  path  with  higher  ordered  approximations.  The  current 
path  generating  program  just  uses  linear  techniques  and  there  may  be 
higher  accuracies  with  more  sophisticated  techniques. 

7.  Using  more  accurate  techniques  for  the  dynamic  and  kinematic 
equations  of  motion.  The  lagrangian  may  not  supply  sufficient  accuracy 
for  higher  d.o.f.  than  3. 

8.  Optimize  the  program  so  that  the  program  operates  faster.  Use  of 
matrices  may  supply  more  speed.  Current  simulation  requires  high  cpu 
time  usage  and  may  prove  a  limiting  factor  for  higher  accuracy  or  more 
complex  path  movements.  For  the  Articulated  Robot  on  a  500  point  path 
the  cpu  time  usage  was  on  the  order  of  10’s  of  minutes. 

9.  Build  and  demonstrate  the  feasibility  of  a  Robot  using  only  position  to 
give  the  desired  accuracy  results,  without  using  lead-through  teaching, 
manual  teaching,  or  complex  programming  techniques. 


This  appendix  contains  a  review  of  basic  trigonometry  and  matrix 
algebra.  Scalars  are  represented  by  lowercase  letters,  vectors  by 
lowercase  bold  letters,  and  matrices  by  uppercase  bold  letters. 


A.1  TRIGONOMETRY 

Right  angle  relationships 

sin  a  =  — 
r 

cos  a  =  — 
r 

tan  a  =  — 
a 


Law  of  Cosines 
2  2  2 

a  =  b  +c  -2bc(cosa) 
2  2  2 

b  =  Q  +c  -2ac(cos  p) 
2 

C  = 


a2  +b2-2ab(cos  Y)  / 


A.2  MATRICES 

Matrix  Multiplication 

A  is  an  n  x  m  matrix  with  components  [aij] 

i  -  1,2,...,n  and  j  -  1,2 . m 

cA  -  Ac  .  [c  aij] 
a(A  +  B)»  aA  +  aB 
(a  +  b)A  »  aA  +  bA 
a(bA)  -  (ab)A 
1(A)»  A 

IA  .  Al  .  A  where  I  is  the  identity  matrix  and  m  .  n 
Given  three  matrices  Amxn,  Bnxp,  and  Cmxp 

Amxn(Bnxp)  -  Cmxp  where  Cij  *  Wk  aik  bkj  ,  k=1 ,2 . n 

(AB)C-A(BC) 

(A+B)C  -  AC  +  BC 
C(A  +  B)  =  CA  +  CB 

If  A  is  a  square  matrix  and  Aij  is  a  cofactor  in  |A| 

AA  1  =  A1  A  .  I 
A  -1  =  [  Aij]1/  |A|  =  adj  A  /  A 
(adj  A)  A  -  A  (  adj  A)  =  |A[  In 
A*1  -  1/|A| 

If  Ai  A2...An  is  the  product  of  square  matrices  then 
(AlA2...An)T  =  An'1  An-1'1.  ..A2"1  Al"1 
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If  AiA2...An  is  conformable  then 

(  AlA2...An  )T  -  (An)T(An-l)T...(A2)T(Al)T 


Translation  by  a  vector  ai  ♦  bj  +  ck 


Trans  (a,b,c)  = 


10  0a 
0  10b 
0  0  1c 
0  0  0  1 


Rotation  about  the  X,  Y,  or  Z  axis 


Rot(x,8)  = 


Rot(y,9)  = 


Rot(z,9)  = 


10  0  0 
0  cos©  -sine  0 
0  sine  cose  0 
0  0  0  1 

cose  0  sine  0 
0  10  0 
-sine  0  cose  0 
0  0  0  1 

cose  -sine  0  0 
sine  cos9  0  0 
0  0  10 
0  0  0  1 


Inverse  Transformation 


nx  ox  ®x  Px 
h  y  Py  ®  y  Py 

nz  °z  *z  ?z 


0  0  0  1 


n x  %  nz 

°x  °z 

ax  ay  az 

0  0  0 


-p*n 

-p*o 

-p-o 


Stretching  Transformation 


0  0  0  0 
0  b  0  0 
0  0  C  0 
0  0  0  1 


Stretches: 

X  by  a 
Y  by  b 
Z  by  c 


Scaling  Transformation 


s  0  0  0 
0  s  0  0 
0  0  s  0 
0  0  0  1 


Paul  (1981,  pp.  9-62)  or  Fu  (1987,  pp.  522-55)  have  good  sections  on  matrix 
operations  as  they  relate  to  Robotics. 
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B.1  DENAVIT-HARTENBERG  NOTATION 

Denavit  and  Hartenberg  (D-H)  proposed  a  system  designed  to  relate 
rotational  and  translational  matrices  between  adjacent  linkages.  Attaching 
a  coordinate  system  to  each  link  the  D-H  representation  results  in  a  4  x  4 
matrix  at  each  joint  and  4x4  matrices  used  to  transform  the  matrices 
between  coordinates.  In  this  way  the  end  effector  in  "tip  coordinates"  can 
be  expressed  in  terms  of  the  "base  coordinates"  by  sequentially  applying  the 
transformation  matrices.  Several  authors  (Paul,  pp.  41-63;  Fu,  pp.  36-41 ; 
Featherstone,  pp.  35-45;  and  Lee,  pp.  68-74)  go  into  some  detail  in  relating 
transformations  and  the  D-H  for  several  robot  models. 

In  Chapter  Three  motion  was  described  using  D-H  techniques  because 
ultimately  the  motion  of  each  joint  will  have  to  be  known  for  object 
avoidance.  Since  we  have  determined  where  we  want  the  tip  or  end  effector 
to  be,  it  would  be  more  convenient  to  use  D-H  matrices  to  reduce  all  of  the 
other  joint  positions.  Since  the  objective  of  this  thesis  was  to  follow  a 
path  without  regard  to  the  joint  positions,  other  than  the  end  effector,  we 
have  ignored  calculating  the  position  of  the  other  joints,  however,  the  D-H 
technique  was  used  to  generate  the  dynamic  equations  for  the  robot  arm 
simulation. 
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B.2  ROBOT  D-H  MATRICES 


There  are  two  types  of  joints,  prismatic  and  rotary,  that  need  to  be 
considered.  In  the  rotary  joint  the  joint  parameters  which  remain  constant 
are  d,  a,  a  while  0  changes  as  the  link  moves  with  respect  to  the  previous 
link  while  for  the  prismatic  joint  the  constant  joint  parameters  are  0,  a, 
and  a,  while  d  is  variable.  Figure  B.1  shows  an  example  of  rotary  and 
prismatic  motion  along  with  the  associated  4x4  matrix.  Two  common 
robots,  the  Stanford  Robot  and  the  PUMA  Robot,  have  been  analysed  in  detail 
by  Paul  (1981 ,  pp.  73-78),  Fu  (1987,  pp.  37-48),  Lee  (1982,  pp.  63-68),  and 
Lee  and  Zeigler  (1984,  pp.  695-705). 

B  3  ARTICULATED  ROBOT  D-H  MATRICES 

Figure  B.2  shows  the  link  coordinates  for  the  3  d.o.f.  articulated  robot 
arm  and  lists  the  link  parameters  and  ranges  of  motion.  The  matrices  are 
given  in  Figure  B.3. 
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PRISMATIC  JOINT 


cos©  -sin©cosa 
sin©  cos0cosa 
0  sin  a 

0  0 


sin©sina  0 
-cos  ©sin  a  0 
cosa  d 


If  0  and  a  are  0.0  then 


An  = 


10  0  0 
0  10  0 
0  0  1  d 
0  0  0  1 


In  General: 
where 


ire  a  is  the  twist  angle  (rotation  around  x) 
0  is  the  angle  rotation  around  z 
a  translation  along  rotated  x 
d  translation  along  z 

An  =  ROT(z,©)TRANS(O,O,d)TRANS(a,O,O)ROT(x,c0 


An  = 


cos©  -sin ©cosa 
sin©  cos  ©cosa 
0  sin  a 

0  0 

If  ©  -  © 

a  *  90 
d  =  0.0 
a  =  0.0 


sin©sina  acos© 
-cos  ©sin  a  asin© 
cosa  d 


then 


An  = 


ROTARY  JOINT 


cos©  0  sin©  0 
sin©  0  -cos©  0 
0  10  0 
0  0  0  1 


Figure  B.1  Rotary  and  Prismatic  Joints. 
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Link  parameters  for  3  d.o.f.  Articulated  Rotot 


Link 

Variable 

0 

a 

a 

d 

1 

@i 

90 

90 

0 

di 

2 

02 

0 

0 

a2 

0 

3 

03 

-90 

90 

0 

d3 

X 


vvvvv.v. 


A,  = 


COS  ©1 

sin  @t 
0 
0 


sin9i 
-cos  01 
0 
0 


A2  = 


COS  02 

sin  ©2 
0 
0 


-sin02 
cos  02 
0 
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T  =  A6  specifies  the  position  and  orientation  of  the 
of  the  Robot  with  respect  to  the  ground.  The  T  matrix 
is  called  the  Arm  Matrix. 


T  =  °T3  \ 


Figure  B.3  Articulated  Robot  D-H  Matrices 


This  appendix  contains  the  Cartesian  Robot  DSL  programs  used  to 
model  the  Cartesian  Robot  Arm.  One  program  is  presented  with  four 
variations  that  were  used. 

ROBSIM2T. FORTRAN  /  RSIM2T6. FORTRAN 

These  programs  are  identical  except  in  the  way  that  the  graphs  are 
plotted.  ROBSIM2T  plots  each  graph  on  a  seperate  page,  while  RSIM2T6 
plots  up  to  six  graphs  on  a  single  page.  Modifications  to  the  Robot 
Simulation  [Kalagarios]  program  allow  for  inputting  multiple  data  runs, 
including  zero  steps,  and  can  be  adapted  for  either  STEP  or  RAMP  Excitation 
as  desired.  Inputs  are  controlled  on  a  position  accuracy  basis. 

ROBSIM2V.  FORTRAN  /  RSIM2V6. FORTRAN 

These  programs  are  identical  to  the  previously  mentioned  programs 
except  in  the  way  that  the  data  points  are  inputted.  Inputs  are  controlled 
on  a  time  basis  and  is  reflected  in  two  line  changes  of  the  ROBSIM2T 
program. 


00000000000000000000 


R0BSIM2T.F0RTRAN  USTING 


TDF: 

C  STEVEN  a  GOODWAY 
C  ROBSIM2T  FORTRAN 

C  NPSWEAPONS  ENGINEERINGELECTRICAL  ENGINEERING 
C  CARTESIAN  ROBOT  SIMULATION  {ROBSIM2T.FORTRAN} 

C  THIS  PROGRAM  TAKES  A  THREE  TUPLE  OF  POINTS  (X.Y.Z)  FROM  A 

C  TABLE  OF  THREE  TUPLES  AND  APPUES  THEM  TO  SIMULATION  OF  A 

C  CARTESIAN  ROBOT.  THE  RESULTING  MOVEMENT  FROM  THE  SIMULATION 
C  IS  THE  EXPECTED  MOVEMENT  OF  THE  TIP  OR  ENDPOINT  OF  THE  ROBOT. 

C  THE  NEXT  THREE  TUPLE  OF  POINTS  ARE  INPUTTED  AFTER  THE  VALUE 

C  OF  THE  ERROR  APPUED  TO  ALL  OF  THE  MOTORS  HAS  DECREASED  TO 

C  BELOW  A  VALUE  CALLED  VALMIN.  THERE  IS  ONE  BASIC  ASSUMPTION 

C  THAT  WAS  MADE  WHEN  CONSTRUCTING  THE  ORIGINAL  THREE  TUPLE  TABLE 

C  AND  WAS  CHOSEN  TO  INSURE  THE  ACCURACY  WHEN  APPLYING  THE  POINTS 

C  TO  THE  SIMULATION. 

C 

C  ASSUMPTIONS: 

C 

1 .  THE  MAXIMUM  ERROR  DURING  ANY  MOVE  IS  LESS  THAN  OR  EQUAL 

TO  THE  DISTANCE  TRAVELED  IN  THAT  DIMENSION  DURING  THE 
STEP. 

F1LEDEFS  REQUIRED  FOR  PROPER  OPERATION  OF  THE  PROGRAM  ARE 
FILEDEF  02  DISK  PATHX  DATA 
FILEDEF  03  DISK  SCOMPX  DATA 

GRAPHS  THAT  RESULT  ARE  SINGLE  PAGE  GRAPHS  (9  TOTAL) 

TIME  RESPONSE  (3) 

PLANE  PROJECTION  (3) 

PATH  ERROR  (3) 


*  SIMULATION  PROGRAM  FOR  CARTESIAN  ROBOT  * 


MAIN  PROGRAM  AREA 

USER  PARAMETER  ADJUSTMENTS 


PARAM  K=1.0,K1  =0.6, K2=1 0000.0, KM1  =59.29, KM2=90.25,KM3=77.44 

PARAM  VSAT =1 50.0, Ml  =0.082,M2=0.041  ,M3=0.041  ,MM=0.186 

PARAM  J1  =0.033, J2=0.033,J3=0.033,R1  =0.91, R2=0.91  ,R3=0.91 

PARAM  KT1  =1 4.4,KT2=14.4,KT3=1 4.4, LI  =0.0001  ,L2=0.0001  ,L3=0.0001 

PARAM  BM1  =0.04297, BM2=0, 04297, BM3=0. 04297, RO=0.5,LOAD=0.0 

PARAM  KV1  =0.1012,KV2=0.1012,KV3=0.1012,T=0.00025,VALMIN=0.001 

PARAM  TV1  =0.0075,TV2=0.0075,TV3=0.0075 

INTEGER  SW1  ,SW2,SW3,N1  ,N2,N3,J 
• 

*  K 1 :  CURVE  SC  AUNG  CONSTANT 


K2:  AMPLIFIER  GAIN 

KM:  IDEAL(MODEL)  MOTOR  CONSTANT 

VSAT:  SATURATION  UMITS  OF  AMPLIFIER 

K:  VELOCITY  LOOP  FEEDBACK  GAIN  (MODEL) 

XPOS.YPOS.ZPOS:  COMMANDED  ENDPOINT  POSITION 

T:  SAMPLING  INTERVAL 

VALMIN:  VALUE  OF  E  USED  TO  CAUSE  NEXT  STEP  TO  BE  INPUTTED. 
H:  NUMBER  OF  POINTS  ALREADY  INPUTTED 

J:  NUMBER  OF  POINTS  IN  PATH  (+1) 


INITIAL 
J*5 
H=0 
SW1-0 
SW2=0 
SW3-0 
N1-0 
N2-0 
N3-0 
RX-0.0 
RY-0.0 
RZ-0.0 
Cl -0.0 
C2-0.0 
C3=0.0 
X1DOT=0. 

X2DOT=0. 

X3DOT=0. 

TL1-0. 

TL2-0. 

TL3-0. 

CF-17RO 
CALL  OPEND  (J) 

CALL  NEXPT  (RX.RY.RZ.C1  .C2.C3) 
DERIVATIVE 
N0S0RT 

A1  -SORT  (2.*KM1  *VSAT) 

A2-SQRT  (2.*KM2*VSAT) 

A3=SQRT  (2.*KM3*VSAT) 
REF1-RX*CF 
REF2=RY*CF 
REF3=RZ*CF 

SORT 

TM1  -Ml  +M2+M3+2*MM+LOAD 

TM2-M2+LOAD 

TM3-M2+M3+MM+LOAD 

JM1-TM1*RO 

JM2*TM2*RO 

JM3-TM3*RO 


JT0T1-J1+JM1 

JTOT2-J2+JM2 

JTOT3-J3+JM3 

*  USE  FOR  STEP  EXCITATION 

*  RTH1 -REF1  *STEP{0.0) 

*  RTH2-REF2*STEP(0.0) 

*  RTH3-REF3*STEP{0.0) 

*  USE  FOR  RAMP  EXCITATION 

RTH1  -(REF1  /TV1  )*(TIME-RAMP(TV1 )) 

RTH2-(REF2/TV2)*(TIME-RAMP(TV2)) 

RTH3-(REF3/TV3)*(TIME-RAMP(TV3)) 

E1-RTH1-C1 

E2-RTH2-C2 

E3-RTH3-C3 


*«*•**•**••*••«***#*••*•****•**•****+••***•*•••«••+•*•••**•• 


NOSORT 


IF  (El  .LT.0.0)  XI  DOT--A1  *K1  *SQRT(ABS(E1 )) 

IF  (El  .GE.0.0)  XI  DOT-  A1  *K1  *SQRT(E1 ) 

IF  (E2.LT.0.0)  X2DOT --A2*K  1  *SQ RT ( ABS( E2)) 

IF  (E2.GE.0.0)  X2DOT-  A2*K1*SQRT(E2) 

IF  (E3.LT.0.0)  X3DOT-A3*K1*SQRT(ABS(E3)) 

IF  (E3.GE.0.0)  X3DOT-  A3*K1*SQRT(E3) 

*  USE  FOR  RAMP  OR  SINE  FUNCTIONS'  POSITION  METHOD 

*  IF  ((ABS(CX-RX).LT.VALMIN).AND.(ABS(CY-RY).LT.VALMIN) ... 

*  .AND.(ABS(CZ-RZ).LT.VALMIN))  THEN 

IF  ((RTH1  .GE.REF1  ).AND.(RTH2.GE.REF2).AND.(RTH3.GE.REF3))THEN 

*  USE  FOR  STEP  FUNCTIONS'  POSITION  METHOD 

IF  ((ABS(E1).LT.VALMIN).AND.(ABS(E2).LT.VALMIN).AND. ... 

(ABS(E3).LT.VALMIN))  THEN 
CALLENDRUN 


USE  FOR  VELOCITY  METHOD  "*****~**ROBSIM2V 
IF  (TIME.GE.((H+1  )*TSTEP))  THEN 

CALLENDRUN 

ENDIF 

ENDIF 


SORT 


KC1  DOT=K*C1  DOT 

XI DOTE-X1 DOT-KC1  DOT 
V1=LIMIT(-VSAT,VSAT,K2*X1  DOTE) 
C1DDT-V1*KM1 
Cl  DOT*INTGRL(0.0,C1  DDT) 

Cl =INTGRL(0.0,C1  DOT) 

cx-criyCF 

VM1  -VI -KV1  *CR1  DOT 

MP1  =REALPL(0.0,L1/R1  .VM1/R1 ) 

MT1-KT1*MP1 

MT1 E-MT1 -BM1  *CR1  DOT-TL1 
CR1  DDT=(1  JJTOT1)*MT1  E 
CR1  DOT=INTGRL(0.0,CR1  DDT) 

CR1  -INTGRL(0.0,CR1  DOT) 
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CRX-CR1*17CF 
XXDOT-XIDOTiyCF 
CXDOT-C1  DOT*1 7CF 
CRXDOT-CR1  DOT*1 7CF 

***•••#•*««*****«•*•«*««**«#«-###**#«««***•*«****•###«#•*«*** 

KC2DOT  «K*C2DOT 

X2DOTE-X2DOT-KC2DOT 

V2«LIMIT(-VSAT,VSAT,K2*X2DOTE) 

C2DDT«V2*KM2 

C2DOT-INTGRL(0.0,C2DDT) 

C2-INTGRL(0.0,C2DOT) 

CY-C2*17CF 

VM2-V2-KV2*CR2DOT 

MP2«REALPL(0.0,L2/R2,VM2/R2) 

MT2-KT2*MP2 

MT2E-MT2-BM2*CR2DOT-TL2 
CR2DDT-(1 7JTOT2)*MT2E 
CR2DOT*INTGRL(0.0,CR2DDT) 
CR2-INTGRL(0.0,CR2DOT) 

CRY«CR2*1 7CF 
XYDOT  *X2DOT*1  VCF 
CYDOT-C2DOT*17CF 
CRYDOT-CR2DOT1  VCF 

*••**♦***  *****<r***********************  *****«***•***••«*•♦*•* 

KC3DOT  «K*C3DOT 

X3DOTE-X3DOT-KC3DOT 
V3-LIMIT  (- VSAT,  VSAT,K2*X3DOTE) 
C3DDT-V3*KM3 
C3DOT-INTGRL(O.O.C3DDT) 
C3-INTGRL(0.0,C3DOT) 

CZ-C3*17CF 

VM3- V3-K  V3*C  R3  DOT 

MP3=REALPL(0.0,L3/R3,VM3/R3) 

MT3=KT3*MP3 

MT3E-MT3-BM3*CR3DOT-TL3 
CR3DDT=(1  yjTOT3)*MT3E 
CR3DOT=INTGRL(0.0,CR3DDT) 
CR3=INTGRL(0.0,CR3DOT) 

CRZ=CR3*1  VCF 
XZDOT=X3DOT*1 7CF 
CZDOT»C3DOT*1 7CF 
C  RZDOT  -CR3DOT*  1 7CF 

#*•**•••••***•**•+****•****•*+************•«•**•***•***#***#  < 

NO0CRF 

IF(ABS(V1  ).LT.  VALM1N)  GOTO  112 
IF  (N1  .EQ.O)  GOTO  1 1 1 
IF  (Cl  DOT.GT.X1  DOT)  SW1=1 
IF  (SW1.EQ.1)  GOTO  222 
KS1  -ABS(2.*CR1  )/(((N1*T)**2)*V1 ) 

KM1-KS1 


SAMPLE 


V.  /.V.  A  /.  <■. 


■  ,  .  A  A  _  ■  .  r.  A  A  A  r.  A  A  ^  A  A  A  A 


222  CONTINUE 

C1-CR1 

C1DOT-CR1DOT 

111  N1-N1+1 

112  CONTINUE 

* 

IF(ABS(V2).LT.VALMIN)  GOTO  334 
IF  (N2.EQ.0)  GOTO  333 
IF  (C2DOT.GT.X2DOT)  SW2-1 
IF  (SW2.EQ.1)  GOTO  444 
KS2«ABS(2.*CR2)/(((N2*T)**2)*V2) 

KM2-KS2 

444  CONTINUE 

C2=CR2 
C2DOT-CR2DOT 

333  N2-N2+1 

334  CONTINUE 

* 

IF(ABS(V3).LT.VALMIN)  GOTO  556 
IF  (N3.EQ.0)  GOTO  555 
IF  (C3DOT.GT.X3DOT)  SW3-1 
IF  (SW3.EQ.1)  GOTO  666 
KS3-ABS(2.*CR3)/(((N3*T)**2)*V3) 

KM3-KS3 

666  CONTINUE 

C3-CR3 
C3DOT-CR3DOT 

555  N3=N3+1 

556  CONTINUE 
SORT 

*♦•*••##♦*##•#*♦*♦#*#•••••****•*•#***#**#•*«#*•#•*##*#****##  TERMINAL 
WRITE  (3,43)  RX.RY.RZ.CX.CY.CZ 
CALL  CERR(RX,RY,RZ,CX,CY,CZ,DC1  ,DC2,DC3) 

43  FORMAT(",T2,6(F10.8,2X)) 

H-H+1 

IF  (H.GE.J-1)  THEN 

RETURN 

ENDIF 

CALL  NEXPT  (RX,RY,RZ,C1  ,C2,C3) 

CALLCONTIN 
METHOD  RKSFX 

CONTRL  FINTIM  -1 0.000, DELT-0.00005  ,DELS=0.00010 
PRINT  0.005  ,H,J,RX,RY,RZ, CX.CY.CZ 
SAVE  (SI)  0.005, RX,RY,RZ,CX,CY,CZ,DC1  ,DC2,DC3 
GRAPH  (G1/S1.DE-TEK618)  TIME(LE»8.0,NU8,UN='SECS'),... 

CX(NI«8,LO— .5,SC=.25,UN«'INCHES') 

GRAPH  (G2/S1.DE-TEK618)  TIME(LE=8.0,NI=8,UN='SECS’),... 

CY(NU8,LO=-.5,SC*.25,UN='INCHES’) 

GRAPH  (G3/S1.DE-TEK618)  TIME(LE=8.0,NI=8,UN='SECS'),... 
CZ(NU8,LO=-.5,SC=.25,UN='INCHES') 
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OOOOOOOOO  0  OOOOOOOo 


GRAPH  (G4/S2.DE-TEK61 8)  CX(LE=8.0,LO*-.25,NU8,UN»,INCHES,)I... 

CY(NI-8,L0— .25, SC-0.25, UN-'INCHES') 

GRAPH  (G5/S2.DE-TEK61 8)  CX(LE=8.0,LO~.25,NI=8, UN-'INCHES'), ... 

C2(NI=8,LO=-.25, SC-0.25, UN-'INCHES') 

GRAPH  (G6/S2.DE-TEK618)  CY(LE=8.0,LO~.25,NI=8, UN-'INCHES'),... 

C2(NI-8,LO— .25, SC-0.25, UN-'INCHES') 

GRAPH  {G7/S1  .DE-TEK61 8)  TIME(UN-'SECS'),... 

DC1  (UN-'ERROR(INCHES)') 

GRAPH  (G8/S1  .DE-TEK61 8)  TIME(UN-'SECS'),... 

DC2  (UN-'ERROR(INCHES)') 

GRAPH  (G9/S1  ,DE=TEK61 8)  TIME(UN-,SECS'),... 

DC3  (UN-'ERROR(INCHES)') 

LABEL  (G1.G2.G3)  TIME  RESPONSE 
LABEL  (G4.G5.G6)  PLANE  PROJECTION 
LABEL  (G7,G8,G9)  PATH  ERROR 
END 
STOP 

C  FORTRAN  SUBROUTINES 
FORTRAN 


******** ******* ******* 

*  SUBROUTINE  OPEND  * 
********************** 


TAKE  THE  VALUE  OF  J  OFF  THE  TABLE  TO  BEGIN  SETTING  UP  FOR 
INPUT  ROUTINE. 

SUBROUTINE  OPEND  (J) 

REWIND  02 
READ  (2,  *)  J 
I  12  F0RMAT(I5) 

RETURN 

BID 


*  SUBROUTINE  NEXTP  * 


INPUTS  THE  NEXT  POINT  WHEN  THE  VALUE  OF  E  DROPS  BELOW  VALMIN 
AND  CHECKS  TO  INSURE  THAT  ALL  OF  THE  VALUES  ARE  NOT  AT  2ERO. 

SUBROUTINE  NEXPT{RX,RY,R2,C1  ,C2,C3) 

67  READ  (02,  *)  RX,RY,R2 

IF  ((RX.LE.0.0).AND.(RY.LE.0.0).AND.(R2.LE.0.0).AND. 

*  (Cl  .LE.0.0).AND.(C2.LE.0.0).AND.(C3.LE.0.0))  THEN 

GOTO  67 

ENDIF 

RETURN 

BID 


ooooo  oooooooo 


SUBROUTINE  CERR 


•  « 


CHECKS  THE  ERROR  BETWEEN  THE  DESIRED  PATH  AND  THE  SIMULATED 
RESULT  IN  THREE  DIMENSIONS 

SUBROUTINE  CERR(RX,RYfRZ,CX,CYICZ,DC1  .DC2.DC3) 

DC1-  (RX-CX) 

DC2-  (RY-CY) 

DC3-  (RZ-CZ) 

RETURN 

B^D 


ft********************************** 

*  END  ROBOT  SIMULATION  SUBPROGRAM  * 
*********************************** 


BOR 
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J*  -A  -«  -«  -A-*. 


r  'm*  S  ~m“ 

%  k.  \  V' 


GRAPH  MODIFICATIONS  TO  ROBSIM2T.FORTRAN  USED  IN  ROBS2T6.FORTRAN. 

*  6  PLOTS  ON  FIRST  PAGE-TIME  RESPONSE  AND  PLANE  PROJECTIONS 

*  3  PATH  ERROR  ON  SECOND  PAGE 

*  XY  PLANE  PROJCTION  AND  3  PATH  ERRORS  ON  THIRD  PAGE 
GRAPH  (Gl/Sl  ,DE-TEK618,PO=1)  TIME(LE=5.0,NI=5,UN=’SEC'),... 

CX(NI-2,LO-0.0, SC-0.5, UN-TNCHES') 

GRAPH  (G2/S1  ,DE=TEK61 8,OV,PO-1 ,2.80)  T1ME(LE-5.0,NI-5,UN-'SEC'),... 

CY(NI-2,LO*0.0,SC-0.5,UN-’INCHES') 

GRAPH  {G3/S1  .DE-TEK61 8.0V.P0-1 ,5.6)  TIME(LE-5.0,NI=5,UN-'SEC'),... 

CZ(NI-2,LO=0.0, SC-0.5, UN='INCHES') 

GRAPH  (G4/Sl,DE-TEK618,OV,PO-8)  CX(LE-2.0,LO-0.0,NI-2,... 

SC-0.5, UN-’SEC'),CY{NI-2,LO-0.0, SC-0.5, UN-'INCHES') 

GRAPH  (G5/S1  .DE-TEK61 8,OV,PO=8,2.80)  CX(LE-2.0,LO-0.0,NI-2,... 

SC-0.5,  UN='SEC),  CZ(NI-2,LO-0.0,  SC-0.5,  UN-INCHES’) 

GRAPH  (G6/S1  .DE-TEK61 8,OV,PO=8,5.60)  CY(LE-2.0,LO-0.0,NI-2,... 

SC-0.5, UN=*SEC*),  CZ(NI=2,LO=0.0,SC-0.5,UN-,INCHES') 

GRAPH  (G7/S1  .DE-TEK61 8,PO=1 )  TIME(LE-5.0,NI-5,UN-,SEC*),... 

DC1  (NI-2  , UN-INCHES') 

GRAPH  (G8/S1  .DE-TEK61 8.0V.P0-1 ,2.80)  TIME(LE-5.0,NI=5,UN-'SEC'),... 
DC2(NI=2  , UN-INCHES') 

GRAPH  (G9/Sl,DE-TEK618,OV,PO-1 ,5.6)  TIME(LE-5.0,NI-5,UN='SEC').... 
DC3(NI=2  ,UN='INCHES') 

GRAPH  (G1 3/SI  .DE-TEK61 8.PO-1 ,1 .75)  CX(LE-5.0,LO-0.00,NI-1 0.SC-.1 .... 

UN-INCHES'),  CY(NI-1 0,LO-0.00,LE-5.00,UN-TNCHES',SC-.1 ) 

GRAPH  (G1 4/SI  .DE-TEK61 8,OV, PO-7.5)  TIME(LE-5.0,NI-5,UN-’SEC'),... 

DC1  (NI-2  .UN-'ERR(IN)') 

GRAPH  (G  15/SI  .DE-TEK61 8,0V, PO-7.5, 2.80)  TIME(LE-5.0,NI-5,UN-'SEC'),... 
DC2(NI-2  ,UN-'ERR(IN)') 

GRAPH  (G1 6/SI  .DE-TEK61 8,0V, PO-7.5, 5.6)  TIME(LE-5.0,NI-5,UN-'SEC'),... 

DC3(NI=2  ,UN='ERR{IN)') 

LABEL  (G1)  TIME  RESPONSE 
LABEL  (G4)  PLANE  PROJECTION 
LABEL  (G7)  PATH  ERROR 
LABEL  (G1 0.G1 1  ,G1 2)  PATH  ERROR 
LABEL  (G13)  XY  PLANE  PROJECTION 
LABEL  (G14)  PATH  ERROR 


This  appendix  contains  the  Articulated  Robot  DSL  programs  used  to 
model  the  Articulated  Robot  Arm.  It  also  contains  the  results  of  the  single 
step  input  used  to  adjust  the  step  timing  for  the  RAMP  Excitations  and  the 
Velocity  Methods. 

RSIMREV1  .FORTRAN 

This  program  adapts  the  Articulated  Robot  Arm  program  (Kalogiros, 
1987)  for  multiple  run  capability,  adjusts  for  zero  input,  and  includes  both 
STEP  and  RAMP  excitation  possibilities.  Designed  for  Position  Method 
control  it  also  takes  care  of  2n  to  0.0  movements.  It  does  not  adjust  for 
paths  whose  Cartesian  Coordinates  pass  through  (0.5,  0.5,  0.5)  which  is  the 
Articulated  Coordinate  System  "home"  position.  Inputted  values  are  a 
series  of  three  tuples  in  Articulated  Coordinates  (CPI  ,CP2,CP3)  which 
follow  a  path  length  integer. 

RSIMREV2.FORTRAN 

This  program  is  the  Velocity  version  of  the  proceeding  program  and 
was  not  ran  to  verify  its  operation.  Modifications  are  supplied  here  only  for 
information  purposes  and  are  noted  in  the  RSIMREV1  program. 


oooooooo 


TOF: 

C  STEVEN  GGOOCWAY 
C  RSIMREV1  .FORTRAN 

C  NPS/WEAPONS  ENGINEERING/ELECTRICAL  ENGINEERING 

C  REVOLUTE  ROBOT  SIMULATION 

C  THIS  PROGRAM  TAKES  THE  THREE  TUPLE  OF  ARTICULATED  COORDINATES 

C  THAT  DESCRIBES  THE  PATH  THAT  THE  ENDOINT  OF  A  ARTICULATED 
C  ROBOT  IS  TO  FOLLOW  AND  APPUES  THE  THREE  TUPLE  TO 

C  A  DSL  PROGRAM  FOR  EACH  ARM  AND  THE  CORRESPONDING  ARM 

C  MOVEMENT  IS  COMPAIRED  TO  THE  ORIGINAL  THREE  TUPLE  FOR 

C  ACCURACY. 

C 

C  FILEDEFS  REQUIRED  IN  'DSLGADD  EXEC*  ARE 
C  FILEDEF  04  DISK  DAPATH  DATA  (RECFM  F 
C  FILEDEF  08  DISK  APATH4  DATA  (RECFM  F 

*  SIMULATION  FOR  ARTICULATED  ROBOT  * 

•♦♦•***•**********##****♦♦*•**•* 


MAIN  PROGRAM  AREA 
USER  PARAMETER  ADJUSTMENTS 

PARAM  K-1 .0.K1  -0.6.K2-1 0000.0, KM1  -0.4225, KM2-0.4225.KM3-4.0 

PARAM  VSAT-1 50.0, Ml  -0.268, M2-0.227.M3-0.041 

PARAM  J1  -0.033, J2-0.033.J3-0.033.R1  -0.91  R2-0.91.R3-0.91 

PARAM  KT1-14.4,KT2-14.4,KT3-14.4,L1  -0.0001, L2-0.0001  .L3-0.0001 

PARAM  BM1  -0.04297.BM2-0.04297.BM3-0.04297, LOAD-O.O 

PARAM  KV1  =0.1 01 2.KV2-0.1 01 2.KV3-0. 1 01 2.T-0.00025. VALMIN-0.001 

PARAM  TV1  -0.0075, TV2-0.0075.TV3-0.0075 

PARAM  D1  -1 5..D2-1 0..D3-1 0..G-386.4 

PARAM  REF1-1  ..REF2-1  ..REF3-1 . 

INTEGER  SW1  .SW2.SW3.N1  ,N2,N3,J 

K 1 :  CURVE  SCALING  CONSTANT 

K2:  AMPLIFIER  GAIN 

KM :  IDEAL(MODEL)  MOTOR  CONSTANT 

VSAT:  SATURATION  UMITS  OF  AMPLIFIER 

K:  VELOCITY  LOOP  FEEDBACK  GAIN  (MODEL) 

CPI  ,CP2,CP3:  COMMANDED  POSITION  IN  RADIANS 

T:  SAMPUNG  INTERVAL 

VALMIN:  VALUE  OF  E  USED  TO  CAUSE  THE  NEXT  STEP  TO  BE  INPUTTED 

*  H:  NUMBER  OF  POINTS  ALREADY  INPUTTED 

J:  NUMBER  OF  POINTS  IN  PATH  (+1 ) 


INITIAL 
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J-5 

H-0 

SW1-0 

SW2-0 

SW3-0 

DC1-0. 

DC2-0. 

DC3-0. 

N1-0 

N2-0 

N3-0 

C1DT-0.0 

C2DT-0.0 

C3DT-0.0 

C1DDT-0.0 

C2DDT-0.0 

C3DDT-0.0 

XI DOT-O. 

X2DOT-0. 

X3DOT-0. 

CR1-0. 

CR2-0. 

CR3-0. 

CR1DT-0. 

CR2DT-0. 

CR3DT-0. 

CR1 DDT-O. 

CR2DDT-0. 

CR3DDT-0. 

TL1-0. 

TL2-0. 

TL3-0. 

MP1-0. 

MP2-0. 

MP3-0. 

MT1-0. 

MT2-0. 

MT3-0. 

CP1-0. 

CP2-0. 

CP3-0. 

CALL  OPEND  (J) 

CALL  FH0ME(C1,C2,C3) 

CALL  NEXPT{H,CP1  ,CP2,CP3,C1  ,C2,C3) 
CI1-CP1 
CI2-CP2 


CI3-CP3 

A1«SQRT(2.*KM1*VSAT) 

A2-SQRT(2.*KM2*VSAT) 

A3-SQRT  (2/KM3*  VSAT) 

DERIVATIVE 

RR1-CP1*STEP(0.0) 

RR2«CP2*STEP{0.0) 

RR3«CP3*STEP{0.0) 

E1-RR1-C1 

E2-RR2-C2 

E3-RR3-C3 

************************************************************  NOSORT 
Dll  -M3*(D2*COS(CR2)+D3*COS(CR2+CR3))**2+M2*D2**2*(COS(CR2))**2 
Dl  1 2-2#((M2+M3)*D2**2*COS(CR2)*SIN(CR2)... 
+M3*D3**2*COS(CR2+CR3)*SIN(CR2+CR3)... 
+M3*D2*D3*SIN(2*CR2+CR3)) 

Dl  13*2*(M3*D3**2*COS(CR2+CR3)*SIN(CR2+CR3)... 

+M3*D2*D3*COS(CR2)*SIN(CR2+C  R3)) 

D22  «(M2+M3)*D2‘*2+M3*D3**2+2*M3*D2*D3*SIN(CR3) 

D23  -M3*D3**2+M3*D2*D3*SIN(CR3) 

D21 1  -(M2+M3)*D2**2*COS(CR2)*SIN(CR2)... 
+M3*D3**2*COS(CR2+CR3)*SIN(CR2+CR3)... 
+M3*D2*D3*SIN(2*CR2+CR3) 

D223-2*M3*D2*D3*SIN(CR3) 

0233-  M3*D2*03*SIN(CR3) 

D32  -  M3*D3**2+M3*D2*D3*COS(CR3) 

D33  -M3‘D3**2 

D31 1  -M3*D3**2*COS(CR2+CR3)*SIN{CR2+CR3)... 

+M3*D2*D3*COS(CR2)*SIN(CR2+CR3) 

D322*M3*D2*D3*SIN(CR3) 

G2  -(M2+M3)*G*D2*COS(CR2)+M3*G*D3*COS(CR2+CR3) 

G3  -M3*G*D3*COS(CR2+CR3) 

TL1  -  -Dl  1 2*CR1  DT*CR2DT-D1 1 3*CR1  DT*CR3DT 
TL2  -  D23*CR3DDT+D21 1  *CR1  DT**2-D33*CR3DT**2-D223*CR2DT*CR3DT 
TL3  -  D32*CR2DDT +D31 1  *CR1  DT**2+D322*CR2DT**2 
JT0T1  -  D11+J1 
JT0T2  -  D22+J2 
JT0T3  -  D33+J3 

IF  (El  .LT.O.O)  XI D0T--A1  *K1  *SQRT(ABS(E1 )) 

IF  (E1.GE.0.0)  XI  DOT-  A1*K1*SQRT(E1) 

IF  (E2.LT.0.0)  X2DOT-A2*K1*SQRT(ABS(E2}) 

IF  (E2.GE.0.0)  X2D0T-  A2*K1*SQRT(E2) 

IF  (E3. LT.O.O)  X3D0T «-A3*K1  *SQRT{ABS(E3)) 

IF  (E3.GE.0.0)  X3D0T-  A3*K1*SQRT(E3) 

*  USE  FOR  RAMP  OR  SINE  FUNCTIONS 


*  IF  ((ABS(C1  -CPI ).LT.VALMIN).AND.(ABS(C2-CP2).LT.VALMIN)  ... 

*  AND.(ABS(C3-CP3).LT.VALMIN))  THEN 

*  IF  {{RR1.GE.CP1).AND.(RR2.GE.CP2).AND.(RR3.GE.CP3))THEN 

*  USE  FOR  STEP  FUNCTIONS'  POSITION  METHOD 

IF  ((ABS(E1).LT.VALMIN).AND.(ABS(E2).LT.VALMIN).AND. .. 

(ABS{E3).LT.VALMIN))  THEN 
CALLENDRUN 

*  USE  FOR  VELOCITY  METHOD  - - RSIMREV2 

*  IF  (TIME.GE.((H+1  )*TSTEP))  THEN 

*  CALLENDRUN 
ENDIF 

*  ENDIF 
SORT 


KC1D0T=K*C1DT 

XI DOTE-X1 DOT-KC1  DOT 

VI  -LIMIT  (-VSATtVSAT,K2*X1  DOTE) 

C1DDT-V1*KM1 

Cl  DT«INTGRL(0.0,C1  DDT) 

Cl -CM  +iNTGRL(0.0,C1  DT) 

VM1-V1 -KV1  *CR1  DT 

MP1«REALPL(0.0,L1/R1  .VM1/R1) 

MT1*KT1*MP1 

MT1 E-MT1 -BM1  *CR1  DT-TL1 

CR1  DDT  »(1 VJTOT 1  )*MT  1 E 

CR1  DT-INTGRL(0.0,CR1  DDT) 

CRI-INTGRL(O.O.CRIDT) 


KC2DOT =K*C2DT 
X2DOTE-X2DOT-KC2DOT 
V2-LIMIT  (- VSAT,  VSAT,  K2*X2DOTE) 
C2DDT-V2*KM2 
C2DT=INTGRL(0.0,C2DDT) 

C2-CI2  +  INTGRL(0.0,C2DT) 

VM2=V2-KV2*CR2DT 

MP2=REALPL(0.0,L2/R2,VM2/R2) 

MT2=KT2*MP2 

MT2E-MT2-BM2*CR2DT-TL2 

CR2DDT=(1 7JTOT2)*MT2E 

CR2DT=INTGRL(0.0,CR2DDT) 

CR2=INTGRL{0.0,CR2DT) 


KC3DOT=K*C3DT 
X3DOTE=X3DOT -KC3DOT 
V3»LIMIT(- VSAT,  VSAT, K2*X3DOTE) 
C3DDT=V3*KM3 


C3DT«INTGRL(0.0,C3DDT) 

C3-CI3  +  INTGRL(0.0,C3DT) 

VM3-V3-KV3*CR3DT 

MP3=REALPL(0.0,L3/R3,VM3/R3) 

MT3=KT3*MP3 
MT3E=MT3-BM3*CR3DT-TL3 
CR3DDT=(1  yJTOT3)*MT3E 
CR3DT=INTGRL(0.0,CR3DDT) 
CR3-INTGRL(0.0,CR3DT) 

..................................... - SAMPLE 

N080RT 

IF(ABS(V1).LT.VALMIN)  GOTO  112 
IF  (N1.EQ.0)  GOTO  111 
IF  (Cl  DT  .GT.X1  DOT)  SW1  =1 
IF  (SW1.EQ.1)  GOTO  222 
KS1  -ABS(2.'CR1  )/({(N1  *T)**2)*V1 ) 

KM1=KS1 

222  CONTINUE 

C1=CR1 
C1DT=CR1DT 

111  N1=N1+1 
CONTINUE 

112  CONTINUE 

IF(ABS(V2).LT.VALMIN)  GOTO  334 
IF  (N2.EQ.0)  GOTO  333 
IF  (C2DT  .GT.X2DOT)  SW2=1 
IF  (SW2.EQ.1)  GOTO  444 
KS2=ABS(2.*CR2)/(((N2*T)**2)*V2) 

KM2=KS2 
444  CONTINUE 

C2=CR2 
C2DT=CR2DT 

333  N2=N2+1 
CONTINUE 

334  CONTINUE 

* 

IF(ABS(V3).LT.VALMIN)  GOTO  556 
IF  (N3.EQ.0)  GOTO  555 
IF  (C3DT  .GT.X3DOT)  SW3=1 
IF  (SW3.EQ.1)  GOTO  666 
KS3=ABS(2.*CR3)/(((N3*T)**2)*V3) 

KM3=KS3 
666  CONTINUE 

C3=CR3 
C3DT*CR3DT 


555 


N3-N3+1 

CONTINUE 

CONTINUE 


556 
SORT 

*********************************«**************************^’£P^|^|||^^|^ 

CALL  CERR  (CPI  ,CP2ICP3,C1  ,C2,C3,DC1  .DC2.DC3) 

WRITE  (4,43)  Cl  ,C2,C3 
43  FORMAT('  ',T2,6(F10.8,2X)) 

H-H+1 

IF  (H.GE.J-1)  THEN 
RETURN 

ENDIF 

CALL  NEXPT(H,CP1  ,CP2,CP3,C1  ,C2,C3) 

CALL  PICRO(CP1  ,CP2,CP3,C1  ,C2,C3) 

CALLCONT1N 
METHOD  RKSFX 

CONTRL  FINTIM  -50.000, DELT-0.00005  ,DELS=0.00010 
PRINT  0.01 0,H  ,CP1,CP2,CP3,  C1,C2,C3 
SAVE  (Si)  0.005, CPI ,CP2,CP3  ,C1,C2,C3,DC1,DC2,DC3 
GRAPH  (G1/S1.DE-TEK618.PO-1)  TIME(LE-5.0,NI-5,UN-,SECS’)I... 

Cl  (NI=2,UN=,RADS‘) 

GRAPH  (G2/S1  .DE-TEK61 8,OV,PO-1 ,2.80/  TIME(LE-5.0,NI-5,UN=,SECS'), 
C2(NI=2,UN='RADS') 

GRAPH  (G3/S1  .DE-TEK61 8.0V.PO-1 ,5.6)  TIMEfLE-S.O.NI-S.UN-'SECS'),.. 
C3(NI-2, UN-RADS') 

GRAPH  (G4/S1  .DE-TEK61 8,PO=1 )  TIME(LE-5.0,NI-5,UN-’SECS'),... 

DC1  (NI-2,UN-'ERR(RADS)') 

GRAPH  (G5/S1  ,DE=TEK61 8,OV, PO-1 ,2.80)  T!ME(LE-5.0,NI-5,UN-'SECS'), 
DC2(NI=2,UN-'ERR(RADS)') 

GRAPH  (G6/S1  .DE-TEK61 8,OV,PO«1 ,5.6)  TIME(LE-5.0,NI-5,UN='SECS'),.. 

DC3(NI=2,UN='ERR(RADS)’) 

LABEL  (Gl )  TIME  RESPONSE 
LABEL  (G4)  PATH  ERROR 
BO 
STOP 

C  FORTRAN  SUBROUTINES 

FORTRAN 

C 

0  ****************** 

C  *  SUBROUTINE  OPEND  * 

C  - 

c 

SUBROUTINE  OPEND  (J) 

REWIND  08 
READ  (08,  *)  J 
REWIND  04 


WRITE(4,*)  J 

RETURN 

BSD 


♦a**************** 

*  SUBROUTINE  FHOME  * 
****************** 


SUBROUTINE  FHOME(C1  ,C2,C3) 
READ(08,*)  Cl  ,C2,C3 
RETURN 
END 


****************** 

*  SUBROUTINE  NEXPT  * 
****************** 


SUBROUTINE  NEXPT(H,CP1  ,CP2,CP3,C1  .C2.C3) 

READ  (08,  *)  CP1,CP2,CP3 

IF  ((CP1,EQ.C1).AND.(CP2.EQ.C2).AND.(CP3.EQ.C3))  THEN 
GOTO  67 

ENDIF 

RETURN 

END 


****************** 

*  SUBROUTINE  PICRO  * 
•*********••*••**• 


CHECKS  FOR  2*PI  CROSSING 

SUBROUTINE  PICRO(CP1  ,CP2,CP3,C1  ,C2,C3) 
Pl=3.141529 

IF  ((Cl.GT.PI).AND.(CP1.LT.(PI/2)))  THEN 
C1=2*PI-C1 

ENDIF 

RETURN 

END 


*  SUBROUTINE  CERR  * 


SUBROUTINE  CERR(CP1  ,CP2,CP3,C1  ,C2,C3,DC1  .DC2.DC3) 
DC1=(CP1-C1) 

DC2-(CP2-C2) 

DC3*(CP3-C3) 


CR2  IRAQ) 


C 1 ,  C2.  and  C3  Step  Response. 


.  ^  K  •  «  4.  ^  f 
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This  appendix  contains  the  main  path  producing  program  and  subroutines. 
It  also  contains  all  of  the  path  modifications  for  each  of  the  motions 
conducted  in  this  thesis.  The  contents  of  this  chapter  are: 

E.1  ROBOT  PATH  PROGRAM  [ROBOPATH. FORTRAN] 

ROBOPATH  is  the  main  program  from  which  all  of  the  paths  are 
generated.  All  of  the  subroutines  are  listed  at  the  end  of  this  program. 

E.2  LINEAR  PATH  1  MAIN  PROGRAM 

E.3  LINEAR  PATH  2  MAIN  PROGRAM 

E.4  CIRCULAR  PATH  MAIN  PROGRAM 

E.5  HELIX  PATH  1  AND  2  MAIN  PROGRAM 

E.6  LINEAR  PATH  PROGRAM 

E.7  SINUSOIDAL  PATH  MAIN  PROGRAM 


oOOOon  o  o  o  r>  tt  o  o  nnonO  O  O  O  o  (5  {5  o  (5 


E.1  ROBOT  PATH  MAIN  PROGRAM 


TOR 

STEVEN  G.  GOODWAY 
R060PATH.F0RTRAN 

NPSWVEAPONS  ENGINEERING/ELECTRICAL  ENGINEERING 
VERSION:: 

RPATH3-  UNEAR  CARTESIAN-  MODIFIED  FROM  VERSION 
ROBPATH7  FORTRAN-PARAMETRIC  SUBROUTINE  ADDED 
CARTESIAN  UNEAR  MOTION 

NPSWVEAPONS  ENGINEERING/ELECTRICAL  ENGINEERING 
CARTESIAN  ROBOT  SIMULATION  {ROBSIM2S.FORTRAN} 

THIS  PROGRAM  CALCULATES  THE  THREE  TUPLE  OF  POINTS  (X,Y,Z) 
THAT  DESCRIBES  THE  PATH  THAT  THE  ENDPOINT  OF  A  CARTESIAN 
ROBOT  IS  TO  FOLLOW.  THEN  THE  THREE  TUPLE  IS  APPUED  TO 
A  DSL  PROGRAM  FOR  EACH  ARM  AND  THE  CORRESPONDING  ARM 
MOVEMENT  IS  COMPAIRED  TO  THE  ORIGINAL  THREE  TUPLE  FOR 
ACCURACY. 

ASSUMPTIONS: 

1 .  THE  MAXIMUM  ERROR  DURING  ANY  MOVE  IS  LESS  THAN  OR  EQUAL 
TO  THE  DISTANCE  TRAVELED  IN  THAT  DIMENSION  DURING  THE 
STEP. 

2.  THE  INPUTED  EQUATIONS  ARE  OF  THE  FORM  Y=F(X)  AND  Z=F(X,Y). 


******%*****♦******«+**# 

*  USER  MODIFICATION  AREA  * 


FUNCTION  EQUAT 1  (X,Y) 
EQUATUX 
RETURN 

END 

2  XY  PROJECTION  FOR  EQUAT1 
FUNCTION  EQATIY(X) 

EQAT1  Y=(1 .5-X) 
RETURN 

END 

C  EQUATION  2 

FUNCTION  EQUAT2(X,Y) 
EQUAT2=1.0 
RETURN 


END 


C  XY  PROJECTION  FOR  EQUAT2 
FUNCTION  EQAT2Y(X) 
EQAT2Y=1.5-X 
RETURN 

END 

C  EQUATION  3 

FUNCTION  EQUAT3(X,Y) 
EQUAT3-1.0 
RETURN 

BJD 

C  XY  PROJECTION  FOR  EQUAT3 
FUNCTION  EQAT3Y(X) 
EQAT3Y=1.5-X 
RETURN 

END 

C  EQUATION  4 

FUNCTION  EQUAT4(X,Y) 
EQUAT4=1 .0 
RETURN 

END 

C  XY  PROJECTION  FOR  EQUAT4 
FUNCTION  EQAT4Y(X) 
EQAT4Y=»1.5-X 
RETURN 

END 

C  PARAMETRIC  X  EQUATION  1 
FUNCTION  PEQX1  (T) 

PEQX1  =  0.75 
RETURN 

END 

C  PARAMETRIC  Y  EQUATION  1 
FUNCTION  PEQY1  (T) 

PEQY1*  (.5+.25*T) 
RETURN 

END 

C  PARAMETRIC  2  EQUATION  1 
FUNCTION  PEQZ1  (T) 
PEQZ1=  0.00 
RETURN 

END 

C  PARAMETRIC  X  EQUATION  2 
FUNCTION  PEQX2(T) 

PEQX2=  {0.75-0.5*T) 
RETURN 

END 

C  PARAMETRIC  Y  EQUATION  2 


FUNCTION  PEQY2(T) 

PEQY2-  0.75 
RETURN 

END 

PARAMETRIC  2  EQUATION  2 
FUNCTION  PEQZ2(T) 

PEQZ2-  0.25 
RETURN 

END 

PARAMETRIC  X  EQUATION  3 
FUNCTION  PEQX3(T) 

PEQX3-  0.25 
RETURN 

END 

PARAMETRIC  Y  EQUATION  3 
FUNCTION  PEQY3(T) 

PEQY3-  0.75-.5*T 
RETURN 

END 

PARAMETRIC  2  EQUATION  3 
FUNCTION  PEQ23(T) 

PEQ23=  0.0 
RETURN 

END 

PARAMETRIC  X  EQUATION  4 
FUNCTION  PEQX4(T) 

PEQX4-  0.25 
RETURN 

END 

PARAMETRIC  Y  EQUATION  4 
FUNCTION  PEQY4(T) 

PEQY4=0.25+0.75*T 

RETURN 

:nd 

PARAMETRIC  2  EQUATION  4 
FUNCTION  PEQ24(T) 

PEQ24®  1.0 
RETURN 
BO 

END  OF  FUNCTION  DESCRIPTION  AREA 

MAIN  PROGRAM  AREA 

USER  PARAMETER  ADJUSTMENTS 

DIMENSION  HSPOS(1 000,3) 

REAL  MAXOD,DELT,MAXODX,MAXODY,MAXOD2,MAXVX,MAXVY,MAXV2 
COMMON  J,I,K,P 


INTEGER  N 

DELT-.1 

MAXOD-.05 

PI-3.141529 
ACC-1  0 

MASS  AND  INERTIAL  ADJUSTMENTS 
MAXVX-1  /MAXOD/DELT 
MAXVY-1  .'MAXOD/DELT 
MAXVZ-1  ‘MAXOD/DELT 
MAXODX-MAXVX*DELT 
MAXODY-MAXWDELT 
MAXODZ-MAXVZ'DELT 
MODIFY  DELT  FOR  PARAMETRIC 
DELT-0.1 

MIN  AND  MAX  LIMITS 
VALMAX-1 E7 
VALMIN=1E-10 

•«•«•**•*•**••****•**•• 

*  EQUATION  BOUNDS  AREA  * 


FIRST  PATH  EQUATION 
START/FINISH  POINTS 
XS5-0.5 
XF5-1.00 

CALCULATE  Y  START/FINISH  POINTS 
YS5-EQAT 1  Y(XS5) 

YF5-EQAT 1  Y(XF5) 

CALCULATE  Z  START/FINISH  POINTS 
ZS5-EQUAT 1  (XS5.YS5) 

ZF5-EQUAT 1  (XF5.YF5) 

SECOND  PATH  EQUATION 
START/FINISH  POINTS 
XS2-0.5 
XF2-1.0 

CALCULATE  Y  START/FINISH  POINTS 
YS2-EQAT2Y(XS2) 

YF2=EQAT2Y(XF2) 

CALCULATE  Z  START/FINISH  POINTS 

ZS2-EQUAT2(XS2,YS2) 

ZF2=EQUAT2(XF2tYF2) 

PARAMETRIC  START  AND  FINISH  POINTS 
FIRST  PARAMETRIC  EQUATION 


>  V  ■  » 


TS-0.0 
TF-1 .00 

XSl-PEQXI(TS) 

YSI-PEQYI(TS) 

ZS1-PEQZ1(TS) 

XF1-PEQX1(TF) 

YF1*PEQY1(TF) 

ZFI-PEQZI(TF) 

C 

C  SECOND  PARAMETRIC  EQUATION 
XS2-PEQX2(TS) 
YS2«PEQY2(TS) 

ZS2«PEQZ2(TS) 

XF2*PEQX2(TF) 

YF2«PEQY2(TF) 

ZF2=PEQZ2(TF) 

C 

C  THIRD  PARAMETRIC  EQUATION 
XS3«PEQX3(TS) 
YS3-PEQY3(TS) 

ZS3«PEQZ3(TS) 


XF3-PEQX3(TF) 

YF3=PEQY3(TF) 

ZF3-PEQZ3(TF) 

FOURTH  PARAMETRIC  EQUATION 
XS4=PEQX4(TS) 

YS4=PEQY4(TS) 

ZS4-PEQZ4(TS) 

XF4=PEQX4(TF) 

YF4-PEQY4(TF) 

ZF4«PEQZ4(TF) 

OUTPUT  START  AND  FINISH  POINTS 

WRITE  (9,65)  T,XSl,YSlpZS1fXF1,YFl,ZF1 
WRITE  (6,65)  T.XS1  ,YS1  ,ZS1  ,XF1  ,YF1  ,ZF1 

WRITE  (9,65)  '2',XS2,YS2,ZS2,XF2,YF2,ZF2 
WRITE  (6,65)  '2',XS2,YS2,ZS2,XF2,YF2,ZF2 

WRITE  (9,65)  '3',XS3,YS3,ZS3,XF3,YF3,ZF3 
WRITE  (6,65)  '3',XS3,YS3,ZS3,XF3,YF3,ZF3 

WRITE  (9,65)  '4',XS4,YS4,ZS4,XF4,YF4,ZF4 
WRITE  (6,65)  ’4\XS4,YS4,ZS4,XF4,YF4,ZF4 


V  V  V  V  V  %  •.  «-  • . 

VVVj  V.V.V.  /  V./ V.  />  /, 


OOnO  OOOOOOOOO  o  o  ooooooo 


WRITE  {9,65)  '5\XS5,YS5,ZS5,XF5,YF5,ZF5 
WRITE  {6,65)  '5,,XS5,YS5,ZS5,XF5,YF5,ZF5 


••a********************** 

*  CALCULATE  PATH  MOVEMENT  * 

ft************* *********** 


HOME 

J-1 

K-1 

HSPOSn,1)=0. 

HSPOS{1 ,2)=0. 

HSPOS{1 ,3)*0. 

WRITE(02,1 81 )  HSPOS(1 ,1  ),HSPOS{1 ,2),HSPOS(1 ,3) 

181  FORMAT('  \T3,3(F10.8,2X)) 

PATH  FORMATION 
XS-XS1 
YS-YS1 
ZS-ZS1 
XF-XF1 
YF-YF1 
ZF=ZF1 

HOME  TO  START 

CALL  HSTAR(XS,YS7S1XF,YF7F,MAX0DX,MAX0DY,MAX0DZ,VAUMIN,J,K) 


*********************** 

*  START  TO  FINISH  EQUAT1  * 
*********************** 


ADJUST  FOR  SLOW  SPEED 
MAXODX=MAXODX/ACC 
MAXODY=MAXODY/ACC 
MAXODZ=MAXODZ/ACC 
N=1 

CALLSTARF(XS,YS,ZS,XF1YFZF,N,MAXODX,MAXODY)MAXODZ,VALMIN 
,J,K) 

START  TO  FINISH  PARAMETRIC  EQUATION  1 

CALLSFPAR{N,TS,TF,DELT,MAXODX,MAXODY,MAXODZ,J,K,VALMIN) 

C  UNEAR  FINISH  EQUAT1  TO  START  EQUAT2-FAST 
C  MAXODX=ACC*MAXODX 
C  MAXODY=ACC*MAXODY 
C  MAXODZ=ACC*MAXODZ 
XS-XF1 


YS-YF1 

ZS-ZF1 

XF-XS2 

YF-YS2 

ZF-ZS2 

CALL  FSTAR(XS)YS,ZS(XF,YF^F,MAXODX,MAXODY,MAXODZ,VALMIN,J,K) 

*********************** 

*  START  TO  FINISH  EQUAT2  * 

**•*****«**•*«• ******** 


MAXODX=MAXODX/ACC 

MAXODY=MAXODY/ACC 

MAXODZ=MAXODZ/ACC 

XS-XS2 

YS-YS2 

ZS-ZS2 

XF-XF2 

YF-YF2 

ZF-ZF2 

N=2 

CALLSTARF(XS,YS,ZS,XF1YF,ZF,N,MAXODX>MAXODY,MAXODZ,VALMIN 

,J,K) 

START  TO  FINISH  PARAMETRIC  EQUATION  2 

CALLSFPAR(N,TSITF,DELT,MAXODX,MAXODY,MAXODZ,JIK,VALMIN) 

UNEAR  FINISH  EQUAT2  TO  START  EQUAT3-FAST 
MAXODX=ACC*MAXODX 
MAXODY=ACC*MAXODY 
MAXODZ=ACC*MAXODZ 
XS-XF2 
YS-YF2 
ZS-ZF2 
XF-XS3 
YF-YS3 
ZF=ZS3 

CALL  FSTAR(XS,YS,ZS,XF1YF,ZF,MAXODX,MAXODY,MAXODZ,VALMIN,J,K) 


*  START  TO  FINISH  EQUAT3  * 


MAXODX=MAXODX/ACC 

MAXOOY=MAXODY/ACC 

MAXODZ=MAXODZ/ACC 


°  °  o  °  OOOO  OOOOo 


>  / 


XS-XS3 

VS-YS3 

ZS-ZS3 

XF-XF3 

YF-YF3 

ZF=ZF3 

N-3 

CALL  STARF(XS,YSrZS,XF,YFrZF,N1MAXODX,MAXODY,MAXODZ,VALMIN 
,J,K) 

START  TO  FINISH  PARAMETRIC  EQUATION  3 
CALLSFPAR(N,TS,TF,DELTtMAXODX)MAXODY,MAXODZ,J,K,VALMIN) 
UNEAR  FINISH  EQUAT3  TO  START  EQUAT4-FAST 
MAXOOX=ACC*MAXODX 
MAXOO  Y=ACC*MAXO  D  Y 
MAXODZ=ACC*MAXODZ 
XS*XF3 
YS-YF3 
ZS-ZF3 
XF-XS4 
YF-YS4 
ZF-ZS4 

CALL  FSTAR(XS,YS1ZS(XF,YF,ZF1MAXODX,MAXODY,MAXODZ,VALMIN,J,K) 


START  TO  FINISH  EQUAT4 


**••»**•*•**« *#««+*«*• 


XS=XF4 

YS=YF4 

2S=ZF4 

XF=XS4 

YF=YS4 

2F=ZS4 

ADJUST  FOR  SLOW  SPEED 
MAXODX=MAXODX/ACC 
MAXODY=MAXODY/ACC 
MAXODZ=MAXOD2/ACC 
N=4 

CALLSTARF(XS,YS,2S.XF,YF2F)N,MAX0DX,MAX0DY,MAX0DZ,VALMIN 

,J,K) 

START  TO  FINISH  PARAMETRIC  EQUATION  4 
CALLSFPAR(N,TS,TF,DELT,MAXODX,MAXODY,MAXODZ,J,KIVALMIN) 
UNEAR  FINISH  EQUAT4  TO  START  EQUAT5-FAST 
MAXODX=ACC*MAXODX 
MAXODY=ACC*MAXODY 


-■  .* 


ooooooo  ooooooooo  oooo  ooooo 


C  MAXODZ*ACC*MAXODZ 
XS-XF4 
YS-YF4 
ZS-ZF4 
XF-XS5 
YF-YS5 
ZF-ZS5 

CALL  FSTAR(XS,YS,ZS,XF,YF)ZF,MAXODX,MAXODY,MAXODZ,VALMIN,J,K) 


*  START  TO  FINISH  EQUAT5  * 


XS-XS5 

YS=YS5 

ZS-ZS5 

XF-XF5 

YF-YF5 

ZF=ZF5 

ADJUST  FOR  SLOW  SPEED 
MAXODX=MAXODX/ACC 
MAXODY=MAXODY/ACC 
MAXODZ=MAXODZ'ACC 
N=1 

CALLSTARF(XS,YS7S,XF1YFtZF,N,MAX0DX,MAX0DY1MAX0DZ,VALMIN 

,J,K) 

START  TO  FINISH  PARAMETRIC  EQUATION  5 

CALL  SFPAR(NITS,TF1DELT,MAXODX,MAXODY,MAXODZ,J,K,VALMIN) 

ADJUST  FOR  FAST  MOVEMENT 
MAXODX=ACC*MAXODX 
MAXODY=ACC*MAXODY 
MAXODZ=ACC*MAXODZ 
FINISH  EQUAT5  TO  HOME 
XF-XF5 
YF*YF5 
ZF*ZF5 

CALL  FHOME(XS,YS7S,XF,YFZF,MAXODX,MAXODY,MAXODZ,VALMIN,J,K) 


*  FINISH  POINT  FORMATION  * 


PRINTOUT  START  AND  FINISH  POINTS 
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WRITE(6,123) 

123  FORMAT{T) 

PRINT  7  * 

PRINT  ‘/ROBOT  SIMULATION  3* 

PRINT  V  ’ 

PRINT  ‘/START  AND  FINISH  POINTS' 

C  COLUMN  HEADERS 

WRITE  (9,45)  'NO//X-START  '/Y-START  '/2-START  ','X-FINISH', 

*  ‘Y-FINISH'/Z-FINISH* 

WRITE  (6,45)  'NO//X-START  ','Y-START  ', '2-START  ','X-FINISH', 

*  'Y-FINISH'/Z-FINISH' 

45  FORMAT  ('  \T2,A3,T6,6(A8,4X)) 

WRITE  (9,65)  '1  ',XS1  ,YS1  ,ZS1  ,XF1 , YF1  ,ZF1 
WRITE  (6,65)  '1  ',XS1  ,YS1  ,ZS1  ,XF1  ,YF1  ,ZF1 
65  FORMAT  ('  ’,T2,A3,6(2X,F1 0.8  )) 

C  READ  DATA 

REWIND  02 
DO  555  I-1.J+1 

READ(2,‘)  HSPOS(l,1),HSPOS(l,2),HSPOS(l,3) 

555  CONTINUE 

C  FORMAT  DATA  FOR  DSL 

REWIND  02 
WRITE  (2,*)  J+1 
DO  553  1-1  ,J+1 

WRITE(2,192)HSPOS(l,1),HSPOS(l,2),HSPOS(l,3) 
553  CONTINUE 

1 92  FORMAT('  ',T3,3(F1 0.8, 2X)) 

C  OUTPUT  VALUES  FOR  THREE  TUPLE 

PRINT  *,' ' 

PRINT  *,' ' 

PRINT  */ THREE  TUPLE  OF  POSITION  POINTS' 

WRITE(9,75)'X-POS'/Y-POS','Z-POS' 

WRITE(6175)'X-POS','Y-POS’,'Z-POS' 

75  FORMAT  ('  ',T3,3(A5,7X)) 

DO  85  1-1  ,J 

WRITE(9,95)  HSPOS(l,1),HSPOS(l,2),HSPOS(l,3) 
WRITE(6.95)  HSPOS(l,1  ),HSPOS(l,2),HSPOS(l,3) 
85  CONTINUE 

95  FORMAT  (”,T2,3(F1 0.7, 2X)) 

PRINT  *,'  ’ 

PRINT  ‘/COMPLETED  POSITION  CALCULATION' 

PRINT  ‘/COMMENCING  ROBOT  ARM  SIMULATION' 

PRINT  *,' ' 

END 
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*  SUBROUTINE  LISTINGS  * 


SUBROUTINE  HSTAR{XS,YS,ZS,XF,YF,ZF,MAXODX,MAXODY, 

*  MAXODZ,VALMIN,J,K) 

CALC  UNEAR  HOME  TO  START  MAXIMIZING  STEEPEST  PATH 
DIMENSION  HSPOS(1 000,3) 

REAL  MAXODX.MAXODY.MAXOOZ.VALMIN 
REWIND  02 

WRITE(9,*)  'HOME  TO  START 
WRITE(9,*)  XS,YS,ZS 
WRITE(9,*)  XF,YF,ZF 
WRITE(9,*)  J,K 
DO  767  1=1  ,J 

READ  (02,*)  (HSPOS(l,L),L=1 ,3) 

767  CONTINUE 
DO  10  J=1,1000 

DELX-MAXODX 
DELY-MAXODY 
DELZ-MAXODZ 
WRITE  (9,*)  DELX.DELY.DELZ 
START  POINT  IS  AT  ZERO 

IF  ((XS.LT.VALMIN).AND.(YS.LT.VALMIN).AND.(ZS.LT.VALMIN))  THEN 
GOT0 100 

ENDIF 


SCALE 

IF  (ZS.GT.XS)  THEN 

GOTO  20 

ENDIF 

IF  (YS.GT.XS)  THEN 

DELX=DELX*(XS/YS) 

DELZ=DELZ*(ZS/YS) 

ELSE 

DELY=DELY*(YS/XS) 

DELZ=DELZ*(ZS/XS) 

ENDIF 
GOTO  30 

20  IF  (ZS.GT.YS)  THEN 

DELX=DELX*(XS/ZS) 

DELY=DELY*(YS/ZS) 

ELSE 

DELX=DELX*(XS/YS) 

DELZ=DELZ*(ZS/YS) 

ENDIF 

HSPOS(J+1 ,1  )=HSPOS(J,1  )+DELX 


30 
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HSP0S(J+1 ,2)«HSPOS(J,2)+DELY 
HSP0S(J+1  ,3)=HSPOS(J,3)+DELZ 

WRITE(9,*)  HSP0S(J+1 ,1  ),HSP0S(J+1  ,2),HSP0S(J+1 ,3) 

C  CHECK  FOR  COMPLETION 

IF((HSPOS(J+1,1).GE.XS).AND.(HSPOS(J+1,2).GE.YS).AND 
*  (HSPOS(J+1 ,3).GE.ZS))  THEN 

GOTO  100 

ENDIF 

C  CHECK  FOR  PUNE  MOVEMENT 

IF  ((HSPOS(J+1,1).GT.XS).AND.(HSPOS(J,1).LT.XS))  THEN 
HSPOS(J+1 ,1  )«HSPOS(J,1 ) 

ENDIF 

IF  ((HSPOS(J+1,2).GT.YS).AND.(HSPOS(J,2).LT.YS))  THEN 
HSPOS{J+1 ,2)=HSPOS(J,2) 

ENDIF 

IF  ((HSPOS(J+1,3).GT.2S).AND,(HSPOS(J,3).LT.ZS))  THEN 
HSPOS(J+1 ,3)x=HSPOS(J,3) 

ENDIF 

10  CONTINUE 

100  HSPOS{J+1.1)-XS 

HSPOS(J+1 ,2)=YS 
HSPOS(J+1 ,3)=ZS 
REWIND  02 
DO  143  I-1.J+1 

WRITE(2,182)  HSPOS(l,1),HSPOS(l,2),HSPOS(l,3) 
143  CONTINUE 

1 82  FORMATC  ,,T3,3(F1 0.8.2X)) 

RETURN 

END 


SUBROUTINE  STARF(XS,YSrZS,XF1YF,ZF,N, 

*  MAXODXIMAXODY,MAXODZ,VALMIN,J,K) 

C  CAUCUUTE  EQUATIONS  FROM  START  TO  FINISH 

DIMENSION  HSPOS(1 000,3) 

REAL  MAXODX.MAXODY.MAXODZ  VALMIN 
REWIND  02 

WRITE(9,*)  ’START  TO  FINISH’ 

DO  768  I-1.J+1 

READ  (02,*)  (HSPOS(l,L),L=1 ,3) 

768  CONTINUE 
DO  11  K=J+1,1000 
DELX=MAXODX 
DELY=MAXODY 
DELZ=MAXODZ 


CHECK  FOR  NEGATIVE  VALUE  DELX.DELY,  OR  DELZ 
IF  ((XF-XS).LT.O.O)  THEN 

DELX-DELX 

ENDIF 

ADJUST  FOR  X  PLANE  MOVEMENT 
IF  (ABS(HSPOS(K,1  )-XF).LE.MAXODX)  THEN 

DELX*(ABS(HSPOS(K,1)-XF)*DELX)/(ABS(DELX)) 

ENDIF 

CALCULATE  DELY  BASED  ON  DELX  OF  MAXODX 
IF  (N.EQ.1)  THEN 

DELY-EQAT1  Y(HSPOS(K,1  )+DELX)-HSPOS(K,2) 

ELSEIF  (N.EQ.2)  THEN 

DELY=EQAT2Y(HSPOS(K,1)+DELX)-HSPOS(K,2) 

ELSEIF  (N.EQ.3)  THEN 

DELY=EQAT3Y(HSPOS(K,1)+DELX)-HSPOS(K,2) 

ELSEIF  (N.EQ.4)  THEN 

DELY«EQAT4Y(HSPOS(K,1)+DELX)-HSPOS(K,2) 

ENDIF 

ADJUST  FOR  PLANE  MOVEMENT  IN  Y  PLANE 
IF  (ABS(DELY).LE.VALMIN)  THEN 
GOTO  51 2 

ENDIF 

ADJUST  FOR  DELY  GREATER  THAN  MAXODY 
WHILE  (ABS(DELY).GT.MAXODY)  DO 

DELX-DELX*(MAXODY/(ABS(DELY))) 

WRITE  (9,*)  DELX,DELY,HSPOS(K,1  ),HSPOS(K,2) 

ADJUST  FOR  PLANE  MOVEMENT  IN  X  DIRECTION 
IF  (ABS(DELX).LE.VALMIN)  THEN 
DELX-0.0 

IF  (ABS(HSPOS(K,2)-YF).GT. MAXODY)  THEN 
DELY=(DELY*MAXODY)/(ABS(DELY)) 

ELSE 

DELY=(DELY*ABS(HSPOS(K,2)-YF))/(ABS(DELY)) 

ENDIF 

WRITE  (9,*)  DELX.DELY 
GOTO  512 

ENDIF 

IF  (N.EQ.1)  THEN 

DELY=EQAT1  Y(HSPOS(K,1  )+DELX)-HSPOS(K,2) 
ELSEIF  (N.EQ.2)  THEN 

DELY=EQAT2Y(HSPOS(K,1)+DELX)-HSPOS(K,2) 
ELSEIF  (N.EQ.3)  THEN 

DELY=EQAT3Y(HSPOS(K,1)+DELX)-HSPOS(K,2) 
ELSEIF  (N.EQ.4)  THEN 

DELY-EQAT4Y(HSPOS(K,1)+DELX)-HSPOS(K,2) 


ENDIF 


END  WHILE 

C  IF  ((YF-YS).LT.O)  THEN 
C  DELY-DELY 
C  ENDIF 
C  CALCULATE  DELZ 
512  IF  {N.EQ.1)  THEN 

DELZ-  (EQUAT1  (HSPOS(K,1  )+DELX,HSPOS(K,2)+DELY)-HSPOS(K,3)) 
ELSEIF  (N.EQ.2)  THEN 

DELZ-  (EQUAT2(HSPOS(K,1)+DELX,HSPOS(K,2)+DELY)-HSPOS(K,3)) 
ELSEIF  (N.EQ.3)  THEN 

DELZ-  (EQUAT3(HSPOS(K,1  )+DELX,HSPOS(K,2)+DELY)-HSPOS(K,3)) 
ELSEIF  (N.EQ.4)  THEN 

DELZ-  (EQUAT4(HSPOS(K,1  )+DELX,HSPOS(K,2)+DELY)-HSPOS(K,3)) 

ENDIF 

C  SCALE  FOR  MAXIMUM  MOVEMENT 
DX-ABS(DELX) 

DY-ABS(DELY) 

DZ-ABS(DELZ) 

C  WRITE  (9,*)  DX.DY.DZ 

IF  ((DZ.GT.DX).AND.(DZ.GT.DY))  THEN 
DELX-DELX*MAXODX/DZ 
DELY-DELY*MAXODY/DZ 
DELZ-DELZ*MAXODZ/DZ 

ENDIF 

C  WRITE  (9,*) '  '  ,DELX,DELY,DELZ 

C  NEXT  POINT 

HSPOS(K+1 ,1  )-HSPOS(K,1  )+DELX 
HSPOS(K+1 ,2)=HSPOS(K,2)+DELY 
HSPOS(K+1 ,3)=HSPOS(K,3)+DELZ 

WRITE(9,183)  HSPOS(K  ,1),HSPOS(K  ,2),HSPOS(K  ,3) 

C  CHECK  FOR  PLANE  MOVEMENT 

IF  (((ABS(HSPOS(K+1 ,1  )-XF)).LT.(ABS(HSPOS(K+1 ,1  )-HSPOS(K,1 )))) 

*  .AND.(ABS(HSPOS(K,1)-XF).LT.MAXODX))  THEN 
HSPOS(K+1,1)=XF 

ENDIF 

IF  (((ABS(HSPOS{K+1 ,2)-YF)).LT.(ABS{HSPOS(K+1 ,2)-HSPOS(K,2)))) 

*  .AND.(ABS(HSPOS(K,2)-YF).LT.MAXODY))  THEN 
HSPOS(K+1 ,2)=YF 

ENDIF 

IF  (((ABS(HSPOS(K+1 ,3)-ZF)).LT.(ABS(HSPOS(K+1 ,3)-HSPOS(K,3)))) 
,AND.(ABS(HSPOS{K,3)-ZF).LT.MAXODZ))  THEN 
HSPOS(K+1 ,3)=ZF 

ENDIF 

C  CHECK  FOR  COMPLETION 

IF  ((ABS(HSPOS(K,1)-XF).LT.MAXODX).AND.(ABS(HSPOS(K,2)-YF).LT. 

*  MAXODY).AND.(ABS(HSPOS(K,3)-ZF).LT.MAXODZ))  THEN 
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GOT0 101 


ENDIF 

CONTINUE 
SAVE  DATA 

REWIND  02 
HSPOS(K+1,1)=XF 
HSPOS(K+1 ,2)=YF 
HSPOS(K+1 ,3)=ZF 
DO  243  I-  1,K+1 

WRITE(2,183)  HSPOS{l,1),HSPOS(l,2),HSPOS(l,3) 
CONTINUE 

FORMATC  ',T3,3(F1 0.8, 2X)) 

RETURN 

END 


SUBROUTINE  FSTAR(XS,YS,ZS,XF,YF,ZF, 

MAXODX,MAXODY,MAXODZ,VALMIN,J,K) 
CALCULATES  UNEAR  FINISH  TO  NEXT  START 
DIMENSION  HSPOS(1 000,3) 

REAL  MAXODX,MAXODY,MAXODZ,VALMIN 
REWIND  02 

WRITE(9,*)  'FINISH  TO  START' 

WRITE(9,*)  XS.YS.ZS 
WRITE(9,*)  XF.YF.ZF 
WRITE(9,*)  J.K+1 
DO  769  I-1.K+1 

READ  (02,*)  (HSPOS(l,L),L=*1,3) 

CONTINUE 
DO  12  J=K+1, 1000 

ALL  START  AND  FINISH  POINTS  THE  SAME 
IF  ((XS.EQ.XF).AND.(YS.EQ.YF).AND.(ZS.EQ.ZF))  THEN 
GOTO  102 

ENDIF 

DELX.MAXODX 
DELY-MAXODY 
DELZ=MAXODZ 
XFS=*  ABS(XS-XF) 

YFS-ABS(YS-YF) 

ZFS-ABS(ZS-ZF) 

IF  (ZFS.GE.XFS)  THEN 
GOTO  22 

ENDIF 

IF  (YFS.GE.XFS)  THEN 

DELX*DELX*(XFS/YFS) 


DELZ-DELZ*(ZFS/YFS) 

ELSEIF  (XFS.GE.YFS)  THEN 

DELY=DELY*(YFS/XFS) 

DELZ=DELZ*(ZFS/XFS) 

ENDIF 

GOTO  32 

22  IF  (ZFS.GE.YFS)  THEN 

DELX*DELX*(XFS/ZFS) 

DELY=DELY*{YFS/ZFS) 

ELSEIF  (YFS.GE.ZFS)  THEN 

DELX=DELX*(XFS/YFS) 

DELZ=DELZ*(ZFS/YFS) 

ENDIF 

C  CHECK  FOR  NEGATIVE  DELX ,  DELY ,  OR  DELZ 

32  IF  ((XF-XS).LT.O)  THEN 

DELX— ABS(DELX) 

ENDIF 

IF  ((YF-YS).LT.O)  THEN 

DELY-ABS(DELY) 

ENDIF 

IF  ((ZF-ZS).LT.O)  THEN 

DELZ— ABS(DELZ) 

ENDIF 

C  NEXT  POINT 

HSPOS(J+1 ,1  )=HSPOS(J,1  )+DELX 

HSPOS(J+1 ,2)=HSPOS(J,2)+DELY 

HSPOS(J+1 ,3)=HSPOS(J,3)+DELZ 

WRITE(9,184)HSPOS(J+1 ,1  ),HSPOS(J+1 ,2),HSPOS(J+1 ,3) 

C  CHECK  FOR  PLANE  MOVEMENT 

IF  (((ABS(HSPOS(J+1 ,1  )-XF)),LT,{ABS(HSPOS(J+1 ,1  )-HSPOS(J,1 )))) 

*  .AND.(ABS(HSPOS(J,1)-XF).LT.MAXODX))  THEN 
HSPOS(J+1,1)=XF 

ENDIF 

IF  (((ABS(HSPOS(J+1 ,2)-YF)).LT.(ABS(HSPOS(J+1 ,2)-HSPOS(J,2)))) 

*  .AND.(ABS(HSPOS(J,2)-YF).LT.MAXODY))  THEN 
HSPOS(J+1,2)=YF 

ENDIF 

IF  (((ABS(HSPOS(J+1 ,3)-ZF)).LT.(ABS(HSPOS(J+1 ,3)-HSPOS(J,3)))) 

*  .AND.(ABS{HSPOS(J,3)-ZF).LT.MAXODZ))  THEN 
HSPOS(J+1,3)=ZF 

ENDIF 

C  CHECK  FOR  COMPLETION 

IF  ((ABS(HSPOS(J,1  )-XF).LE.ABS{DELX)).AND.(ABS(HSPOS(J,2)-YF) 

*  .LE.ABS(DELY)).AND.(ABS(HSPOS(J,3)-ZF).LE.ABS(DELZ)))  THEN 
GOTO  102 


ENDIF 


12  CONTINUE 

102  HSPOS(J+1 ,1)«XF 

HSPOS(J+1 ,2)-YF 
HSPOS(J+1 ,3)-ZF 
C  SAVE  DATA 
REWIND  02 
DO  343  I-  1.J+1 

WRITE(2,184)  HSPOS(l,1),HSPOS(l,2),HSPCS(l,3) 

C  WRITE(9,184)  HSPOS(l,1),HSPOS(l,2).HSPOS(l,3) 

343  CONTINUE 

1 84  FORMAT(’  \T3,6(F1 0.8.2X)) 

RETURN 

END 

C 

SUBROUTINE  FH0ME(XS,YS(ZS,XF,YF7F, 

*  MAXODX,MAXODY,MAXODZ,VALMIN,J,K) 

C  CALC  UNEAR  FINISH  TO  HOME  MAXIMIZING  STEEPEST  PATH 
DIMENSION  HSPOS(1 000,3) 

REAL  MAXODX.MAXODY.MAXODZ.VALMIN 
REWIND  02 

WRITE(9,*)  'FINISH  TO  HOME  ' 

DO  766  1-1  ,K+1 

READ  (02.*)  (HSPOS(I,L),L=1,3) 

766  CONTINUE 
DO  13  J-K+1,1000 

DELX-MAXODX 

DELY-MAXODY 

DELZ-MAXODZ 

C  FINISH  POINT  IS  AT  ZERO 

IF  ((XF.LT.VALMIN).AND.(YF.LT.VALMIN).AND.(ZF.LT.VALMIN))  THEN 
GOT0 103 

ENDIF 

C 

IF  (ZF.GE.XF)  THEN 

GOTO  23 

ENDIF 

IF  (YF.GE.XF)  THEN 

DELX»DELX*(XF/YF) 

DELZ=DELZ*(ZF/YF) 

ELSE 

DELY-DELY*(YF/XF) 

DELZ-DELZ*(ZF/XF) 

ENDIF 
GOTO  33 

23  IF  (ZF.GE.YF)  THEN 

DELX=DELX*(XF/ZF) 
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DELY«DELY*{YF/ZF) 

PI  SF 

DELX=DELX*(XF/YF) 

DELZ=DELZ*(ZF/YF) 

ENDIF 

33  HSP0S(J+1,1)-HSP0S(J,1)-DELX 

HSP0S(J+1 ,2)=HSPOS(J.2)-DELY 
HSP0S(J+1 ,3)=HSPOS(J,3)-DELZ 

WRITE(9,185)  HSP0S(J+1 ,1),HSP0S(J+1 ,2),HSP0S(J+1 ,3) 

C  CHECK  FOR  COMPLETION 

IF((HSPOS(J+1 ,1  ).LE.O.).AND.(HSPOS(J+1 ,2).LE.0.).AND. 

*  (HSPOS(J+1.3).LE.O.))THEN 

GOT0 103 

ENDIF 

C  CHECK  FOR  PLANE  MOVEMENT 

IF  ((HSPOS(J+1 ,1  ).LT.XF) AND.(HSPOS(J,1  ).GT.XF))  THEN 
HSPOS(J+1 ,1  )*HSPOS(J,1 ) 

ENDIF 

IF  ((HSPOS(J+1,2).LT.YF).AND.(HSPOS(J,2).GT.YF))  THEN 
HSPOS(J+1 ,2)*HSPOS(J,2) 

ENDIF 

IF  ((HSPOS(J+1,3).LT.ZF)AND.(HSPOS(J,3).GT.ZF))  THEN 
HSPOS(J+1 ,3)-HSPOS(J,3) 

ENDIF 

13  CONTINUE 

103  HSPOS(J+1 ,1)=0. 

HSPOS(J+1 ,2)-0. 

HSPOS(J+1 ,3)-0. 

C  SAVE  DATA 

REWIND  02 
DO  4431=  1.J+1 

WRITE(2,185)  HSPOS(l,1),HSPOS(l,2),HSPOS(l,3) 

C  WRITE(9,185)  HSPOS(l,1),HSPOS(l.2),HSPOS(l.3) 

443  CONTINUE 

1 85  FORMATC  \T3,3(F1 0.8, 2X)) 

RETURN 

END 

C  PARAMETRIC  SUBROUTINE  SFPAR 

SUBROUTINE  SFPAR(N.TS,TF,DELT,MAXODX,MAXODY>MAXODZ,J,K,VALMIN) 
DIMENSION  HSPOS(  1000.3) 

REAL  MAXODX,MAXODY,MAXODZ,VALM!N,DELT,TS,TF 
DT-DELT 
REWIND  02 
DO  810  I-1.J+1 

READ  (2,*)(HSPOS(l,L),L=1 ,3) 

810  CONTINUE 


K-J+1 

WHILE  (T.LT.TF)  DO 

820  IF  (N.EQ.1)  THEN 

DELX-PEQX1  (T+DT)-PEQX1(T) 

ELS  El  F  (N.EQ.2)  THEN 

DELX=PEQX2(T+DT)-PEQX2(T) 

ELSEIF  (N.EQ.3)  THEN 

DELX-PEQX3(T+DT)-PEQX3(T) 

ELSEIF  (N.EQ.4)  THEN 

DELX=PEQX4(T+DT)-PEQX4(T) 

ENDIF 

IF  (ABS(DELX).GT.MAXODX)  THEN 

DT-ABS(DT*MAXODX/DELX) 

GOTO  820 

ELSEIF  ((ABS(DELX).LE.VALMIN).OR.(DT.LE.VALMIN))THEN 
DELX-0.0 
DT-DELT 

ENDIF 

DTT-DT 

830  IF  (N.EQ.1)  THEN 

DELY=PEQY1  (T+DT)-PEQY1  (T) 

ELSEIF  (N.EQ.2)  THEN 

DELY-PEQY2(T+DT)-PEQY2(T) 

ELSEIF  (N.EQ.3)  THEN 

DELY«PEQY3(T+DT)-PEQY3(T) 

ELSEIF  (N.EQ.4)  THEN 

DELY=*PEQY4(T+DT)-PEQY4(T) 

ENDIF 

IF  ( ABS(D ELY) .GT .M AXOD Y)  THEN 

DT -ABS(DT*M  AXOD  Y/DELY) 

GOTO  830 

ELSEIF  ((ABS(DELY).LE.VALMIN).OR.(DT.LE.VALMIN))THEN 
DELY=0.0 
DT=DTT 

ENDIF 

DTT-DT 

840  IF  (N.EQ.1)  THEN 

DELZ-PEQZ1  (T+DT)-PEQZ1  (T) 

ELSEIF  (N.EQ.2)  THEN 

DELZ=PEQZ2(T+DT)-PEQZ2(T) 

ELSEIF  (N.EQ.3)  THEN 

DELZ=PEQZ3(T+DT)-PEQZ3(T) 

ELSEIF  (N.EQ.4)  THEN 

DELZ-PEQZ4(T +DT)-PEQZ4(T) 


ENDIF 


IF  (ABS(DELZ).GT.MAXODZ)  THEN 
DT=ABS(DT*MAXODZ/DELZ) 

GOTO  840 

ELSEIF  ((ABS(DELZ).LE.VALMlN).OR.(DT.LE.VALMIN))THEN 
DELZ-0.0 
DT-DTT 

ENDIF 

C  MAXED  OUT  YET 

IF  ((T+DT).GT.TF)  THEN 
DT-TF-T 

ENDIF 

C  CALCULATE  NEXT  POINT 
IF  (N.EQ.1)  THEN 

DELX-PEQX1  (T+DT)-PEQX1  (T) 

DELY.PEQY1  (T+DT)-PEQY1  (T) 

DELZ-PEQZ1  (T+DT)-PEQZ1  (T) 

ELSEIF  (N.EQ.2)  THEN 

DELX=PEQX2(T +DT)-PEQX2(T) 
DELY«PEQY2(T+DT)-PEQY2(T) 
DELZ=PEQZ2(T+DT)-PEQZ2(T) 

ELSEIF  (N.EQ.3)  THEN 

DELX=PEQX3(T+DT)-PEQX3(T) 

DELY*PEQY3(T +DT)-PEQY3(T) 
DELZ-PF.QZ3(T+DT)-PEQZ3(T) 

ELSEIF  (N.EQ.4)  THEN 

DELX=PEQX4(T +DT)-PEQX4(T) 

DELY=PEQY4(T +DT)-PEQY4(T) 
DELZ=PEQZ4(T+DT)-PEQZ4(T) 

ENDIF 

C  WRITE  (9,1 1 5)  DT,DELX,DELY,DELZ 

1 1 5  FORMAT  (•  ’,T3,4(F6.4,3X)) 

HSPOS(K+1,1)*HSPOS(K,1)+DELX 
HSPOS(K+1 ,2)=HSPOS(K,2)+DELY 
HSPOS(K+1 ,3)=HSPOS(K,3)+DELZ 

WRITE(9,1 83)  HSPOS(K,1)tHSPOS(K,2),HSPOS(K,3) 

T=T+DT 
K-K+1 
END  WHILE 

C  SAVE  RESULTS 
REWIND  02 
00  243  1=  1.K 

WRITE(2,183)  HSPOS(l,1),HSPOS(l,2),HSPOS(l,3) 

C  WRITE(9,183)  HSPOS(l,1),HSPOS(l,2),HSPOS(l,3) 

243  CONTINUE 

K-K-1 

183  FORMAT('  ’,T3,3(F10.8,2X)} 
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E 2  LINEAR  PATH1  MODIFICATIONS 


STEVEN  a  GOOD  WAY 

RPATH2-  UNEAR  CARTESIAN-  MODIFIED  FROM  VERSION 


*  USER  MODIFICATION  AREA  * 


FUNCTION  EQU AT  1  (X,Y) 
EQUAT 1  =  .500 
RETURN 

END 

XY  PROJECTION  FOR  EQUAT1 
FUNCTION  EQATIY(X) 
EQAT1Y=1.25-X 
RETURN 

END 

EQUATION  2 

FUNCTION  EQUAT2(X,Y) 
EQUAT2=1 .0 
RETURN 

END 

XY  PROJECTION  FOR  EQUAT2 
FUNCTION  EQAT2Y(X) 
EQAT2Y=1.5-X 
RETURN 

END 

EQUATION  3 

FUNCTION  EQUAT3(X,Y) 
EQUAT3=1.0 
RETURN 

END 

XY  PROJECTION  FOR  EQUAT3 
FUNCTION  EQAT3Y(X) 
EQAT3Y=1 ,5-X 
RETURN 

END 

C  EQUATION  4 

FUNCTION  EQUAT4(X,Y) 
EQUAT4=1.0 
RETURN 

END 

i  XY  PROJECTION  FOR  EQUAT4 
FUNCTION  EQAT4Y(X) 
EQAT4Y=1.5-X 


RETURN 


END 

PARAMETRIC  X 
FUNCTION  PEQX(T) 

PEQX=  .5*COS(T)+.5 
RETURN 
END 

PARAMETRIC  Y 
FUNCTION  PEQY(T) 

PEQY*  .5*SlN(T)+.5 
RETURN 
BO 

PARAMETRIC  Z 
FUNCTION  PEQZ(T) 

PEQZ=  T/(1 2*3.1 41 529) 

RETURN 

END 

END  OF  FUNCTION  DESCRIPTION  AREA 


*  EQUATION  BOUNDS  AREA  * 

*********************** 

FIRST  PATH  EQUATION 
START/FINISH  POINTS 
XS1-1. 

XF1=0.25 

CALCULATE  Y  START/FINISH  POINTS 
YS1  =EQAT  1  Y(XS1 ) 

YF1=EQAT1Y(XF1) 

CALCULATE  Z  START/FINISH  POINTS 
ZS1  =EQUAT  1  (XSl  ,YS1 ) 

ZF1 =EQUAT  1  (XF1  ,YF1 ) 

OUTPUT  START  AND  FINISH  POINTS  FOR  EQUATION  1 

WRITE  (9,65)  '1  \XS1  ,YS1  ,ZS1  ,XF1  ,YF1  ,ZF1 

WRITE  (6,65)  '1  \XS1  ,YS1  ,ZS1  ,XF1  ,YF1  ,ZF1 

SECOND  PATH  EQUATION 

START/FINISH  POINTS 

XS2-0.5 

XF2=1.0 

CALCULATE  Y  START/FINISH  POINTS 
YS2= EQ  AT2Y  (XS2) 

YF2=EQAT2Y(XF2) 

CALCULATE  Z  START/FINISH  POINTS 
ZS2=EQUAT2(XS2,YS2) 

ZF2=EQUAT2(XF2,YF2) 

OUTPUT  START  AND  FINISH  POINTS  FOR  EQUATION  2 


OOOq  Q  OOOO 


WRITE  (9,65)  '2',XS2,YS2,ZS2,XF2,YF2,ZF2 
WRITE  (6,65)  '2',XS2tYS2,ZS2,XF2,YF2,ZF2 


*************************** 


•  CALCULATE  PATH  MOVEMENT  * 


PATH  FORMATION 

XS-XS1 

YS=YS1 

ZS-ZS1 

XF=XF1 

YF=YF1 

ZF=ZF1 

HOME  TO  START 

CALL  HSTAR(XS,YS^S^F1YFZF,MAXODX,MAXODY1MAXODZ,VALMIN,J,K) 


*********************** 


*  START  TO  FINISH  EQUAT1  * 


N-1 


CALLSTARF(XS,YS7S,XF1YFZF,N,MAX0DX,MAX0DY,MAX0DZ,VALMIN 

,J,K) 

LINEAR  FINISH  EQUAT1  TO  START  EQUAT2-FAST 

XS=XF1 

YS=YF1 

ZS=ZF1 

XF-XS2 

YF*YS2 

ZF=ZS2 

CALLFSTAR(XS,YS,ZS1XF,YF7F,MAX0DX,MAX0DY,MAX0DZ,VALMIN,J,K) 
START  TO  FINISH  EQUATION  2 
XS=XS2 
YS=YS2 
ZS=ZS2 
XF=XF2 
YF=YF2 
ZF=ZF2 
N=2 

CALLSTARF(XS,YS7S,XF,YF7F,N,MAX0DX,MAX0DY,MAX0DZ)VALMIN 

,J.K) 

FINISH  EQUAT2  TO  HOME 

XF=XF2 

YF=YF2 

ZF=ZF2 

CALL  FH0ME(XS,YS7S,XF,YF7F,MAX0DX,MAX0DY,MAX0DZ,VALMIN,J,K) 
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'j.Txr.XT.y 


c 

c 

c 


************************ 

*  FINISH  POINT  FORMATION 

•♦A******************** 


END  MODIRCATION  REGION 


E.3  UNEAR  PATH  2  PROGRAM 


DESCRIBED  IN  SECTION  E.1 


E4  CIRCULAR  PATH  MOTION 


:  STEVEN  O  GOOD  WAY 

:  ROBPATH4  FORTRAN 

">  **************************** 

2  *  USER  MODIFICATION  AREA  * 

■N  **************************** 

«/ 

FUNCTION  EQUAT 1  (X,Y) 

EQUAT 1  =0.5 
RETURN 

END 

:  XY  PROJECTION  FOR  EQUAT1 

FUNCTION  EQATIY(X) 

EQAT 1  Y=(SQRT{ABS(.25-(X-.5)**2)))+.5 
RETURN 

END 

FUNCTION  EQUAT2(X,Y) 

EQUAT2=0.5 

RETURN 

END 

:  XY  PROJECTION  FOR  EQUAT2 

FUNCTION  EQAT2Y(X) 

EQAT2Y=-(SQRT(ABS(.25-(X-.5)**2)))+.5 

RETURN 

END 

3  END  OF  FUNCTION  DESCRIPTION  AREA 

•N 

:  MAIN  PROGRAM  AREA 

D  USER  PARAMETER  ADJUSTMENTS 


*  EQUATION  BOUNDS  AREA  * 


FIRST  PATH  EQUATION 
START/FINISH  POINTS 
XS1=1. 

YS1=0.5 

XF1=0.0 

YF1=0.5 

CALCULATE  Z  START/FINISH  POINTS 
ZS1  =EQUAT  1  (XS1  ,YS1 ) 

ZF1  =EQUAT  1  (XF1  ,YF1 ) 


SECOND  PATH  EQUATION 


ewuiucii 


START/FINISH  POINTS 

XS2-0.0 

YS2-0.5 

XF2-1.0 

YF2-0.5 

CALCULATE  Z  START/FINISH  POINTS 

ZS2«EQUAT2(XS2,YS2) 

ZF2=EQUAT2(XF2,YF2) 


*  CALCULATE  PATH  MOVEMENT 

•  ***■**«***••*«#*****•****•• 


PATH  FORMATION 

XS=XS1 

YS=YS1 

ZS=ZS1 

XF-XF1 

YF*YF1 

ZF=ZF1 

HOME  TO  START 

CALL  HSTAR(XS,YS,ZS,XF,YFIZF,MAXODX,MAXODY,MAXODZ,VALMIN,J>K) 


*  START  TO  FINISH  EQUAT 1  * 

•■ft********************* 

ACC=0.01 

ADJUST  FOR  SLOW  SPEED 

MAXODX=MAXODX/ACC 

MAXODY=MAXODY/ACC 

MAXODZ=MAXODZ^ACC 

N=1 

CALLSTARF(XS,YS,ZS,XF1YF)ZF1N1MAXODX,MAXODY,MAXODZ,VALMIN 

,J.K) 

LINEAR  FINISH  EQUAT  1  TO  START  EQUAT2-FAST 

MAXODX=ACC*MAXODX 

MAXOD  Y=ACC*MAXO  D  Y 

MAXODZ=ACC*MAXODZ 

XS=XF1 

YS*YF1 

ZS=ZF1 

XF-XS2 

YF-YS2 

ZF=ZS2 

CALL  FSTAR(XS,YS,ZS,XF,YF,ZFIMAXODX,MAXODY,MAXODZ,VALMIN,J,K) 
START  TO  FINISH  EQUATION  2 


.V  O  V  o  >  *  ■ 


•-.V.V.V.V'/  ^ 
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I 


MAXOOX-MAXOOX/ACC 

MAXOOY-MAXODY/ACC 

MAXOD2=MAXODZ/ACC 

XS-XS2 

YS-YS2 

2S-2S2 

XF-XF2 

YF-YF2 

2F-2F2 

N-2 

CALLSTARF(XS,YS,2S,XF,YF,ZFIN,MAX0DX,MAX0DY,MAX0D2.VALMIN 
*  ,J,K) 

ADJUST  FOR  FAST  MOVEMENT 

MAXODX=-ACC*MAXODX 

MAXODY=ACC*MAXODY 

MAXOD2=ACC*MAXOD2 

FINISH  EQUAT3  TO  HOME 

XF-XF2 

YF-YF2 

2F-2F2 

CALL  FHOME{XF,YF,2F,MAXODX,MAXODY,MAXOD2,VALMIN,J,K) 


*  FINISH  POINT  FORMATION  * 

ft********************* 
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E.5  HELIX  PATH  1  AND  2 


TOP: 

C  STEVEN  G.  GOOD WAY 

C  ROBPATH5  FORTRAN-PARAMETRIC  SUBROUTINE  ADDED 

C  N PS/WEAPONS  ENGINEERING/ELECTRICAL  ENGINEERING 

c  ...................... 

C  *  USER  MODIFICATION  AREA  * 

Q  A*********************** 

FUNCTION  EQUAT 1  (X,Y) 

EQUAT1=0.5 

RETURN 

END 

C  XY  PROJECTION  FOR  EQUAT1 
FUNCTION  EQATIY(X) 

EQAT 1  Y=(SQRT  (ABS(.25-(X-.5)**2)))+.5 
RETURN 

BO 

FUNCTION  EQUAT2(X,Y) 

C  EQUAT2=((ABS(.25-(X-.5)**2  -(Y-.5)**2  ))**. S+.5) 

EQUAT2=0.5 
RETURN 

END 

C  XY  PROJECTION  FOR  EQUAT2 
FUNCTION  EQAT2Y(X) 

EQAT2Y=-(SQRT(ABS(.25-(X-.5)**2)))+.5 

RETURN 

END 

C  PARAMETRIC  X 

FUNCTION  PEQXfT) 

PEQX=  ,5*COS(T)+.5 

RETURN 

END 

C  PARAMETRIC  Y 

FUNCTION  PEQY(T) 

PEQY=  .5*SIN(T)+.5 

RETURN 

END 

C  PARAMETRIC  Z 

FUNCTION  PEQZ(T) 

C  HELIX  3-CYCLE 

C  PEQZ=T/(6*3.141529) 

C  HELIX  6-CYCLE 

PEQZ=T/(1 2*3.1 41 529) 

RETURN 

END 
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END  OF  FUNCTION  DESCRIPTION  AREA 

MAIN  PROGRAM  AREA 
USER  PARAMETER  ADJUSTMENTS 
ACC=1 .0 

MODIFY  DELT  FOR  PARAMETRIC 
DELT=0.1 

*•***+«••«•*#********** 

*  EQUATION  BOUNDS  AREA  * 


FIRST  PATH  EQUATION 

PARAMETRIC  START  AND  FINISH  POINTS 

TS=0.0 

HELIX  3-CYCLE 
TF=6*PI 
HELIX  6-CYCLE 
TF=12*PI 
XS1=PEQX(TS) 

YS1=PEQY(TS) 

ZS1=PEQZ(TS) 

XF1*PEQX(TF) 

YF1=PEQY(TF) 

ZF1=PEQZ(TF) 

********««*********++**** 

*  CALCULATE  PATH  MOVEMENT  * 

•*•*«•«**•*«•***+******** 

PATH  FORMATION 

XS=XS1 

YS=YS1 

ZS=ZS1 

XF=XF1 

YF=YF1 

ZF=ZF1 

HOME  TO  START 

CALLHSTAR(XS.YS,ZS,XF,YF,ZF,MAXODX,MAXODYIMAXODZ,VALMIN,J,K) 

*•***#+•****«***•*•*#*• 

*  START  TO  FINISH  EQUAT1  * 


START  TO  FINISH  PARAMETRIC  EQUATION 
CALL  SFPAR(TS,TF,DELT1MAXODX,MAXODY,MAXODZ,J,K1VALMIN) 
FINISH  EQUAT3  TO  HOME 
XF=XF1 
YF=YF1 
ZF=ZF1 

CALL  FHOME(XF,YF,ZF,MAXODX,MAXODY,MAXODZ,VALMIN,J,K) 


ooooo 


***** ******* *********** 


*  FINISH  POINT  FORMATION  * 


END  HEUX  PATH  1  &  2  MODIFICATION 
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E.6  UNEAR  PATH  MODIFICATIONS 


TOF: 

STEVH^  a  GOODWAY 

ROBPATH6  FORTRAN-PARAMETRIC  SUBROUTINE  ADDED 
UNEAR  PARAMETRIC/CATRSIAN 

a*********************** 

*  USER  MODIFICATION  AREA  * 

************************ 

PARAMETRIC  X  -EQUATION  1 
FUNCTION  PEQX1{T) 

PEQX1=  1. 

RETURN 
END 

PARAMETRIC  Y  -EQUATION  1 
FUNCTION  PEQY1(T) 

PEQY1  =  .25+T 

RETURN 
END 

PARAMETRIC  Z  -EQUATION  1 
FUNCTION  PEQZ1  (T) 

PEQZ1-.5 

RETURN 
BO 

PARAMETRIC  X  -EQUATION  2 
FUNCTION  PEQX2(T) 

PEQX2=  ,75-T 

RETURN 
END 

PARAMETRIC  Y  -EQUATION  2 
FUNCTION  PEQY2(T) 

PEQY2= 1 . 

RETURN 
END 

PARAMETRIC  Z  -EQUATION  2 
FUNCTION  PEQZ2(T) 

PEQZ2=  .5 

RETURN 
END 

PARAMETRIC  X  -EQUATION  3 
FUNCTION  PEQX3(T) 

PEQX3=  0.0 

RETURN 
END 

PARAMETRIC  Y  -EQUATION  3 
FUNCTION  PEQY3(T) 


PEQY3-  1. 


RETURN 

END 

PARAMETRIC  Z  -EQUATION  3 
FUNCTION  PEQZ3(T) 

PEQZ3-  2.*T 

RETURN 

END 

CARTESIAN  -EQUATION  4 
FUNCTION  EQUAT4(X,Y) 

EQUAT4-Y 

RETURN 

END 

XY  PROJECTION  FOR  EQUAT4 
FUNCTION  EQAT4Y(X) 

EQAT4Y=*1.  -  X 
RETURN 

END 

END  OF  FUNCTION  DESCRIPTION  AREA 

MAIN  PROGRAM  AREA 

USER  PARAMETER  ADJUSTMENTS 


*  EQUATION  BOUNDS  AREA  * 


PARAMETRIC  START  AND  FINISH  POINTS 

TS=0.0 

TF-0.5 

PARAMETRIC  EQUATION  1  START/FINISH 
XSI-PEQXI(TS) 

YS1=PEQY1(TS) 

ZSI-PEQZI(TS) 

XFI-PEQXI(TF) 

YFI-PEQYI(TF) 

ZF1=PEQZ1(TF) 

PARAMETRIC  EQUATION  2  START/FINISH 
XS2=PEQX2(TS) 

YS2=»PEQY2(TS) 

ZS2=PEQZ2(TS) 

XF2=PEQX2(TF) 

YF2=PEQY2(TF) 

ZF2=PEQZ2(TF) 

PARAMETRIC  EQUATION  3  START/FINISH 
XS3=PEQX3(TS) 

YS3=PEQY3(TS) 

ZS3=PEQZ3(TS) 


oooo  o  ooooo 


*  CALCULATE  PATH  MOVEMENT  * 


PATH  FORMATION 
XS-XS1 
YS-YS1 
ZS-ZS1 
XF-XF1 
YF-YF1 
ZF-ZF1 

HOME  TO  START 

CALL  HSTAR(XS,YSrZS,XF,YFrZF,MAXODX,MAXODY,MAXODZ,VALMIN,J,K) 


*  START  TO  FINISH  EQUAT1  * 

•*•*•******•*«•*••**«*• 

START  TO  FINISH  PARAMETRIC  EQUATION 
N=1 

CALLSFPARCTS.TF.DELT.MAXODX.MAXODY.MAXODZJ.K.VALMIN.N) 
UNEAR  FINISH  EQUAT1  TO  START  EQUAT2 
XS=XF1 
YS*YF1 
ZS=ZF1 
XF=XS2 
YF=YS2 
ZF=ZS2 

WRITE  (9,*)  XS,YS,ZS,XF,YF,ZF 

CALL  FSTAR(XSIYS,ZS,XF,YF,ZF,MAXODX,MAXODY,MAXODZ,VALMIN,J,K) 
START  TO  FINISH  EQUATION  2 
XS=XS2 
YS=YS2 
ZS=ZS2 
XF=XF2 
YF=YF2 
ZF=ZF2 
N=2 

CALLSFPAR(TS,TF,DELT,MAXODX,MAXODY,MAXODZ,J,K,VALMIN,N) 
LINEAR  FINISH  EQUAT2  TO  START  EQUAT3 
XS=XF2 
YS=YF2 
ZS=ZF2 
XF=XS3 
YF=YS3 
ZF=ZS3 

CALL  FSTAR(XS,YS,ZS,XF,YF,ZFIMAXODX,MAXODY,MAXODZ,VALMIN,J,K) 
START  TO  FINISH  EQUATION  3 
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N-MN  HI  PATH  FOLLOWING  ROBOT(U)  NAVAL  POSTGRADUATE  SCHOOL 
HONTEREV  CA  S  G  GOODHAV  DEC  (7 


UNCLASSIFIED 


F/0  12/9 


NL 


oooo 


I  SM4.M .HI.'  .  IC- iH  Ala  im 


XS-XS3 

YS-YS3 

ZS-ZS3 

XF-XF3 

YF-YF3 

ZF-ZF3 

N-3 

CALLSFPAR{TS,TF,DELT,MAXODX,MAXODY,MAXODZJ,K,VALMIN,N) 
UNEAR  FINISH  EQUAT2  TO  START  EQUAT3 
XS-XF3 
YS-YF3 
ZS-ZF3 
XF-XS4 
YF-YS4 
ZF-ZS4 

CALL  FSTAR(XSiYS,ZS^F1YFZF,MAXODX,MAXODY,MAXODZ,VALMINJ,K) 
START  TO  RNISH  EQUATION  4 
XS-XS4 
YS-YS4 
ZS-ZS4 
XF-XF4 
YF.YF4 
ZF-ZF4 
N-4 

CALL  STARF(XS,YS,ZS,XF,YF,ZFfNI 
MAXODX,MAXODY,MAXODZ,VALMINJ,K) 

FINISH  EQUAT4  TO  HOME 
XF-XF4 
YF-YF4 
ZF=ZF4 

CALL  FHOME(XF,YF,ZFIMAXODX,MAXODY,MAXODZ,VALMIN,J(K) 


*********************** 


*  FINISH  POINT  FORMATION 
*********************** 


WWW 
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E.7  SINUSOIDAL  PATH  MODIFICATIONS 


TOF: 

STEVBg  a  GOODWAY 

ROBPATH7  FORTRAN-PARAMETRIC  SUBROUTINE  ADDED 

*  USER  MODIFICATION  AREA  * 

FUNCTION  EQUATI(X.Y) 

EQUAT 1  -ABS{COS(9*3.1 41 52 9*X)) 

RETURN 

BSD 

XY  PROJECTION  FOR  EQUAT1 
FUNCTION  EQAT1Y(X) 

EQAT 1  Y«(ABS{SIN(6*3 .141 529*X))) 

RETURN 

END 

END  OF  FUNCTION  DESCRIPTION  AREA 

MAIN  PROGRAM  AREA 
USER  PARAMETER  ADJUSTMENTS 

ft********************** 

•EQUATION  BOUNDS  AREA  * 

*«********************* 


FIRST  PATH  EQUATION 
START/FINISH  POINTS 
XS1-1. 

XF1-0.0 

CALCULATE  Y  START/FINISH  POINTS 

YSI-EQATIY(XSI) 

YFI-EQATIY(XFI) 

CALCULATE  Z  START/FINISH  POINTS 
ZS1  -EQUAT  1  (XS1  ,YS1 ) 

ZF1-EQUAT1(XF1,YF1) 

************************** 

*  CALCULATE  PATH  MOVEMENT  * 
•*«*•  ****•**+*«•**•****•« 


PATH  FORMATION 
XS-XSl 
YS-YS1 
ZS-ZS1 

XF-XF1 

YF-YF1 

ZF-ZF1 

289 


HOME  TO  START 

CALL  HSTAR(XS,YS,ZS,XF1YFtZF,MAXODX,MAXODYIMAXODZ,VALMIN,J,K) 
*  START  TO  FINISH  EQUAT1  * 


N-1 

CALLSTARF{XS,YSfZS,XF,YF,ZF,N,MAXODX,MAXODYIMAXODZfVALMIN 

J,K) 

HNISH  EQUAT3  TO  HOME 

CALL  FHOM  E{XF,  YF,ZF,MAXODX,MAXOD  Y,  MAXODZ,  VALM  IN  J  ,K) 


ft********************** 

*  FINISH  POINT  FORMATION  * 
*#•*•»********••••*#*•• 


290 


-  r.v. 


% 


This  appendix  contains  the  3-Dimensional  plotting  program.  In  the 
current  listing  the  plane  projections  will  not  be  drawn.  For  plane 
projections  just  enable  all  three  CURVE  functions.  A  reminder  that  large 
data  points  will  require  a  lot  of  memory,  upwards  to  40%  of  user  A-disk, 
and  that  parts  of  the  axis  may  be  ignored  by  the  DISSPLA  routine.  Format  of 
the  data  points  are  a  integer  J,  which  represents  one  less  then  the  total  of 
the  three  tuples  in  table  form,  which  follow. 


TOF: 

C  STEVBSI  a  GOODWAY 

C  ROBCOMP1  .FORTRAN 

C  NPS/WEAPONS  ENGINEERINGELECTRICAL  ENGINEERING 

C  CARTESIAN  ROBOT  SIMULATION  {ROBSIM1  .FORTRAN} 

C  THIS  PROGRAM  TAKES  THE  THREE  TUPLE  OF  POINTS  (X,Y,Z) 

C  THAT  DESCRIBES  THE  PATH  THAT  THE  END°OINT  OF  A  CARTESIAN 
C  ROBOT  IS  TO  FOLLOW  AND  PLOTS  THEM  IN  THREE  DIMENSIONS. 

C 

C 

C  MAIN  PROGRAM  AREA 

C  USER  PARAMETER  ADJUSTMENTS 

DIMENSION  POS{1 000,3),X(1 000),Y(1 000), Z(1 000) 

C 

Q  ******************** 

C  *  3-D  PLOT  SUBROUTINE  * 

Q  ******************** 

C 


INTEGER  N,l 
CALL  PAGE(11., 8.5) 


o  o 


GET  DATA 
REWIND  02 
READ  (2,*)  J 
DO  90 1-1.J-1 

READ(2,*)(POS(l,L),L-1 ,3) 

X(l)«POS(l,1) 

Y(l)-POS(l,2) 

Z(l)«POS(l,3) 

i  CONTINUE 

OUTPUT  TO  TEK  618 
CALL  TEK618 
CALL  NOBRDR 

SET  UP  Z-AXIS  PARALLEL  TO  X-Y  PLANE 
CALL  ZAXANG(-90) 

SET  UP  TITLE  AND  PLOT  AREA 
CALL  AREA2D(7.,7.) 

CALL  HEADINC3D  PATH  TRAJECTORY  $',100,2.,2) 
CALL  HEADIN('ROBOT  PATH  MOTIONS  $’,1 00,1 .5,2) 
SET  UP  AXIS  LABELS  AND  DEFINES  3D  WORKSPACE 
CALL  VOLM3D(  7. ,7. ,7.) 

CALL  X3NAME('X-POS',5) 

CALL  Y3NAME(’Y-POS\5) 

CALL  Z3NAME('Z-POS',5) 

DEFINE  VIEWPOINT 
CALL  VUABS(-30.,-10.,23.) 

DEFINE  THE  AXIS  SYSTEM 
CALL  GRAF3D(0.0,'SCALE',1  .0,0.0,'SCALEM  .0,0.0, 

*  'SCALE', 1.0) 

3-D  CURVE  PLOTTING 
CALL  POLY3 

CALL  CURV3D(X,Y,Z,J,0) 

PUT  2-D  PLOT  ON  FLOOR  OF  3-D 
CALL  GRFITI(0.,0.,0.,1  .,0.,0.,0.,1  .,0.) 

CALL  AREA2D(7.,7.) 

CALL  GRAF(0.0, 'SCALE' ,  1 .0,0.0,'SCALEM  .0) 

CALL  CURVE(X,Y,J,0) 

PUT  2-D  PLOT  OF  XZ  DATA  ON  BACK  WALL  OF  3-D 
CALL  GRFITI(0.,7.,0.,1  .,7.,0.,1  .,7., 2.) 

CALL  XNAME('X-POS',5) 

CALL  AREA2D(7.,7.) 

CALL  GRAF(0.0, 'SCALE', 1 .0,0.0,'SCALE',1 .0) 

CALL  CURVE(X,Z,J,0) 


*  O'*  '  4.*  «  "  *■_ 1 
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PUT  20  PLOT  OF  YZ  DATA  ON  OTHER  BACK  WALL 
CALLGRFITI{7.,7.,0.,7.f5.,0.,7.,5.f5.) 

CALL  XNAME('Y-P0S’,5) 

CALL  YNAMECZ-POS',5) 

CALL  AREA2D(7.,7.) 

CALL  G  RAF(0.0, 'SCALE' ,  1 .0,0.0,'SCALEM  .0) 

CALL  CURVE(Y,Z,J,0) 

TERMINATE  PLOT 
CALL  ENDPL(O) 

CALL  DONEPL 


This  appendix  contains  the  coordinate  transformation  programs 
necessary  to  convert  from  Cartesian  Coordinates  to  Articulated  Coordinates 
and  back  to  Cartesian  Coordinates,  with  intermediate  transformation 
through  spherical  coordinate  system. 

TOR 

C  STEVEN  GOODWAY 

C  WEAPONS  SYSTEMS  ENGINEERING/ELECTRICAL  ENGINEERING 

C  TRANSFORM  FORTRAN 

C  THE  PURPOSE  OF  THIS  PROGRAM  IS  TO  PERFORM  COORDINATE 

C  TRANSFORMATIONS  AND  TO  PERFORM  MISCILANEOUS  FUNCTIONS 

C  SUCH  AS  SC  AUNG  AND  UMITING  FUNCTIONS.  THE  COORDINATES 

C  TRANSFORMATIONS  ARE: 

C 


c 

FROM: 

TO: 

SUBROUTINE: 

c 

CARTESIAN 

SPHERICAL 

CARSP 

c 

SPHERICAL 

CARTESIAN 

SPCAR 

c 

SPHERICAL 

ARTICULATED 

SPART 

c 

ARTICULATED 

SPHERICAL 

ARTSP 

c 

THE  MISCELLANEOUS  SPECIAL  FUNCTIONS  ARE: 

c 

CARTESIAN  SCALING 

[CARSC] 

c 

SPHERICAL  SC  AUNG 

[SPHSC] 

c 

CARTESIAN  LIMITING 

[CARLI] 

c 

SPHERICAL  UMITING 

[SPHLI] 

C 

C  TO  OPERATE  THE  PROGRAM  A  PATH  OF  SUBROUTINES  MUST  BE  GENERATED 
C  THAT  IS  IN  THE  ORDER  THAT  THE  USER  INTENDS  TO  PERFORM  IN. 

C  ALSO  THE  USER  MUST  DECLARE  SEVERAL  FILEDEFS  SO  THAT  THE  FLOW 

C  IS  PROPER. 

C 

C  THE  FILEDEFS  FOR  CARTESIAN  TO  ARTICULATED  ARE  PRIOR  TO 

C  SIMULATION: 


C 

UNIT 

SAMPLE 

TYPE 

c 

02 

PATH4 

DATA 

c 

03 

SCOMP4 

DATA 

c 

04 

COMP4 

DATA 

05 

SPATH4 

DATA 

09 

TERM 

14 

APATH4 

DATA 

15 

ASPATH4 

DATA 

16 

SCPATH4 

DATA 

THE  FILEDEFS  FOR  ARTICULATED  TO  CARTESIAN  AFTER  SIMULATION  ARE: 


UNIT 

SAMPLE 

TYPE 

09 

TERM 

14 

DAPATH4 

DATA 

16 

DCPATH4 

DATA 

*  MAIN  USER  PATH  FORMATION 
************************** 

CARTESIAN  TO  SPHERICAL 
REAL  MAX 
MAX-1.0 
SMAX-0.5 
SF-1.0 
CALLCARU 


C 

CALL  CARSC(SF.MAX) 

CC 

CALLCARU 

CC 

CALL CARSP 

CCCC  SPHERICALTO  ARTICULATED 

CC 

SF-1.0 

C 

CALLSPHLI 

C 

CALL  SPHSC(SF,SMAX 

CC 

CALLSPHU 

CC 

CALLSPART 

CCCC 

ARTICULATED  TO  SPHERICAL 
CALLARTSP 

G00C  SPHERICALTO  CARTESIAN 

CC 

SF-1/SMAX 

C 

STOP 

END 

CALL  USPSC(SF) 
CALLSPCAR 

0  ******* 

C  *  SUBROUTINES  * 

0  ******* 

C 

/■*  ******* 

*  CARSP  SU8RUTINE 
***************** 


C  THIS  PROGRAM  TRANSFORMS  FROM  CARTESIAN  TO  SPHEREICAL 

C  COORDINATE  SYTEMS. 

C  POS(l,L)  -  PATH  POSITION  IN  CARTESIAN 
C  SPPOS(l,L)  -  PATH  POSITION  IN  SPHERICAL 
C  HOME  FOR  THE  CARTESIAN  ROBOT  IS  (0.0.0.0.0.0)  HOWEVER 
C  HOME  FOR  THE  ARTICULATED  ROBOT  (SPHERICAL)  IS  (0.5, 0.5, 0.5). 
C  IF  INTENDING  TO  TRANSFORM  INSURE  THAT  THE  ORIGINAL  PATH 

C  STARTS  AT  THE  SPHERICAL  HOME  POSITION. 

SUBROUTINE  CARSP 
INTEGER  l,J,K,L 

DIMENSION  POS(1000,3),SPPOS(1000,3) 

REWIND  02 

C  INPUT  CARTESIAN  PATH 

READ(2,*)  J 
DO  300 1-1.J-1 
READ(2,*)(POS(l,L),L=1 ,3) 

300  CONTINUE 

C  CALCULATIONS  FROM  CARTESIAN  TO  SPHERICAL 

DO  400  1-1, J-1 
C  CALCRHO 

SPPOS(l,  1  )«(SQRT  ((POS(l,1  )-0.5)**2+(POS(l,2)-0.5)**2+( 

*  POS(I,3)-0.5)**2)) 

C  CALC  THETA  FROM  XY  PROJECTION 

B«(SQRT((POS(l,1  )-.5)**2+(POS(l,2)  -.5)**2)) 

A-(SQRT((POS(l,1)-1  .)**2+(POS(l,2)  -.5)**2)) 

C-.5 

PI-3.1 41 5926536 
IF  (B.EQ.0.0)  THEN 

SPPOS(I,2)-0.0 

ELSE 

SPPOS(  I,2)=ACOS((B**2+C**2-A**2)/(2.*C*B)) 

IF  (POS(I,2).LT.0.5)  THEN 

SPPOS(l,2)-  2  *  PI  -SPPOS(l,2) 

ENDIF 

ENDIF 

C  CALC  PHI  IN  THREE  DIMENSIONS 

F-(SQRT(B**2+(POS(l,3)-1  .)**2)) 

R-SPPOS(I.I) 

IF  (R.EQ.0.0)  THEN 

SPPOS(I,3)=0.0 

ELSE 

SPPOS(l,3)=(ACOS((C**2+R**2-F**2)/(2.*C*R))) 

ENDIF 

400  CONTINUE 

C  PRINT  AND  I/O  STATEMENTS 


WRITE(9,*)  J,’  CARTESIAN  TO  SPHERICAL' 
WRITE(5,*)  J 
DO  500  1-1  ,J-1 

WRITE  (5,*)(SPPOS(l,L),L-1 ,3) 

500  CONTINUE 

742  FORMAT  {'  ',T2,14,2X,6{F5.3,2X)) 

RETURN 

END 


C  *  SPART  SUBROUTINE  * 

Q  «M*****M«**t*M* 

c 

C  THIS  PROGRAM  TRANSFORMS  FROM  SPHERICAL  TO  ARTICULATED 

C  COORDINATE  SYTEMS.  THE  SPHERICAL  COORDINATE  SYSTEM  IS  COMPOSED 

C  THE  THREE  TUPLE  (RHO,THETA,PHI)  AND  THE  ARTICULATED  SYSTEM 

C  IS  COMPOSED  OF  THE  THREE  TUPLE  (CPI  ,CP2,CP3)  WHICH  DISCRBES 

C  THE  COMANDED  ANGULAR  POSITION  FOR  THE  THREE  ROBOT  MOTIONS  AND 

C  EXPRESSED  IN  RADIANS.  TO  PROPERLY  WORK  THE  SPHERICAL  SYSTEM 
C  MUST  EXIST  WITHIN  A  1  UNIT  DIAMETER  SPHERE. 

SUBROUTINE  SPART 
INTEGER  I.J.K.L 

DIMENSION  ARTPOS(1 000,3),SPPOS(1 000,3) 

C  INPUT  CARTESIAN  PATH 
REWIND  5 
READ(5,*)  J 
DO  301  I-1.J-1 

READ(5,*)(SPPOS(!,L),L=1 ,3) 

301  CONTINUE 

C  CALCULATIONS  FROM  CARTESIAN  TO  SPHERICAL 

Pl=3.1 41 5926536 
DO  401  1=1  ,J-1 
C  CALCULATE  CPI 

ARTPOS(l,1  )=SPPOS(l,2) 

C  CALCULATE  CP2 

ARTPOS(l,2)=PI-SPPOS(l,3)+ACOS(2*SPPOS(l,1)) 

C  CALCULATE  CP3 

ARTPOS(l,3)*PI-2*(ACOS(2*SPPOS(l,1))) 

401  CONTINUE 

C  PRINT  AND  I/O 

REWIND  14 

WRITE(9,*)  'SPHERICAL  TO  ARTICULATED  (RIT,PICP1,CP2,CP3), 
WRITE(14,*)  J 
DO  501  1=1  ,J-1 

WRITE  (14,*)(ARTPOS(l,L),L=1 ,3) 

501  CONTINUE 


oO 


V.V^V.V  IT*  .-X  k-> 


742  FORMAT  (*  ,,T2,I4,2X,6(F5.3,2X)) 

RETURN 
END 


*#*•4*************** 


*  ARTSP  SUBROUTINE  * 

4*4*44444444444*4*44 


C  THIS  PROGRAM  TRANSFORMS  FROM  ARTICULATED  TO  SPHERICAL 
C  COORDINATE  SYTEMS.  THE  SPHERICAL  COORDINATE  SYSTEM  IS  COMPOSED 
C  THE  THREE  TUPLE  (RHO, THETA, PHI)  AND  THE  ARTICULATED  SYSTEM 

C  IS  COMPOSED  OF  THE  THREE  TUPLE  (CPI  ,CP2,CP3)  WHICH  DESCRIBES 

C  THE  COMANDED  ANGULAR  POSITION  FOR  THE  THREE  ROBOT  MOTIONS  AND 

C  EXPRESSED  IN  RADIANS.  TO  PROPERLY  WORK  THE  SPHERICAL  SYSTEM 

C  MUST  EXIST  WITHIN  A  1  UNIT  DIAMETER  SPHERE. 

SUBROUTINE  ARTSP 
INTEGER  l,J,K,L 

DIMENSION  ARTPOS(1 000,3), SPPOS(1 000,3) 

C  INPUT  CARTESIAN  PATH 
REWIND  14 
READ{14,*)  J 
DO  302  I-1.J-1 

READ(14,*)(ARTPOS(l,L),L=1 ,3) 

302  CONTINUE 

C  CALCULATIONS  FROM  ARTICULATED  TO  SPHERICAL 

PI-3.1415926536 
DO  402  I-1.J-1 
C  CALCULATE  RHO 

SPPOS(l,1)-.5*COS((PI-ARTPOS(l,3))/2) 

C  CALCULATE  THETA 

SPPOS(l,2)=ARTPOS(l,1 ) 

C  CALCULATE  PHI 

SPPOS(l,3)=PI-ARTPOS(l,2)+(ACOS(2*SPPOS(l,1))) 

402  CONTINUE 

C  PRINT  AND  I/O 

REWIND  15 

WRITE(9,*)  ’ARTICULATED  TO  SPHERICAL  (CPI  ,CP2,CP3,R,T,P)' 
WRITE(15,*)  J 
DO  502  I-1.J-1 

WRITE  (15,*)(SPPOS(l,L),L=1 ,3) 

502  CONTINUE 

742  FORMAT  (’  ’,T2,I4,2X,6(F5.3,2X)) 

RETURN 

END 


-,v.v 


*  SPCAR  SUBROUTINE  * 

****************** 

THIS  PROGRAM  TRANSFORMS  FROM  SPHERICAL  TO  CARTESIAN 
COORDINATE  SYTEMS.  THE  SPHERICAL  COORDINATE  SYSTEM  IS  COMPOSED 
THE  THREE  TUPLE  (RHO,THETA,PHI)  AND  THE  CARTESIAN  SYSTEM 
IS  COMPOSED  OF  THE  THREE  TUPLE  (X,Y,Z)  WHICH  DESCRIBES 
THE  COORDINATES  THAT  WERE  SIMULATED  BY  THE  ROBOT  MOTION. 
SUBROUTINE  SPCAR 
INTEGER  l,J,K,L 

DIMENSION  POS(1 000, 3),SPPOS(1 000,3) 

INPUT  CARTESIAN  PATH 
REWIND  15 
READ(15,*)  J 
DO303I-1.J-1 

READ(15,*)(SPPOS(l,L),L=1 ,3) 

CONTINUE 

CALCULATIONS  FROM  SPHERICAL  TO  CARTESIAN 
DO  403  1-1  ,J-1 
CALCULATE  X 

R«SPPOS(l,1)*SIN<SPPOS(l,3)) 

POS(l,1)=.5+R*COS(SPPOS(l,2)) 

CALCULATE  Y 

POS(l,2)=.5+R*SIN(SPPOS(l,2)) 

CALCULATE  Z 

POS(I,3)=.5+SPPOS(I,1)*COS{SPPOS(I,3)) 

CONTINUE 
PRINT  AND  I/O 

REWIND  16 

WRITE(9,*)  'SPHERICAL  TO  CARTESIAN  (R,T,P,X,Y,Z)' 
WRITE(16,*)  J 
DO  503  1-1  ,J-1 

WRITE  (16,*)(POS(l,L),L=1 ,3) 

CONTINUE 

FORMAT  ('  ,,T2,I4,2X,6(F5.3,2X)) 

RETURN 

END 

****************************** 

*  SCALE  IN  CARTESIAN  COORDNATES  * 


THE  PURPOSE  OF  THIS  PROGRAM  IS  TO  TAKE  A  SET  OF  CARTESIAN 
COORDINATE  THREE  TUPLE  AND  SCALE  SO  THAT  THE  LARGEST  VALUE 
IS  ON  THE  SURFACE  OF  A  (1 ,1 ,1 )  CUBE.  THIS  INSURES  THAT  THE 
MAXIMUM  ATTEMPTED  MOVEMENT  IS  WITHIN  THE  RANGE  OF  THE  MOTION 
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505 

743 

455 

END 


OF  THE  ROBOT  ARM  WHEN  THIS  "NORMALIZED"  VALUE  IS  USED  WITH 
A  SCALING  FACTOR  (SF).  WITH  SF  UESS  THAN  OR  EQUAL  TO  ONE  THEN 
THE  MOTION  WILL  BE  WITHIN  THE  UNIT  CUBE.  A  SF  GREATER  THAN 
ONE  CAN  BE  USED  WITH  A  CARTESIAN  ROBOT  ARM  WHICH  REQUIRES 
INPUTS  TO  BE  SCALED  TO  THE  ARM  LENGTHS.  WHEN  USED  IN 
CONJUNCTION  WITH  THE  SPHERICAL  PROGRAM  THE  CARTESIAN  COORD 
MUST  BE  WITHIN  THE  UNIT  CUBE. 

SUBROUTINE  CARSC(SF,MAX) 

INTEGER  l,J,K,L 
REAL  SF, MAX 
DIMENSION  POS(1 000,3) 

REWIND  2 

INPUT  CARTESIAN  PATH 
READ(2,*)  J 
DO  305  1*1  ,J-1 
READ(2,*)(POS(l,L),L=1 ,3) 

IF  (POS(I.I).GT.MAX)  THEN 

MAX=POS(l,1) 

ENDIF 

IF  (POS(l,2).GT.MAX)  THEN 

MAX-POS(l,2) 

ENDIF 

IF  (POS(l,3).GT.MAX)  THEN 

MAX-POS(l,3) 

ENDIF 

CONTINUE 

SCALE 

IF  (MAX.EQ.0.0)  THEN 

GOTO  455 

ENDIF 

DO  405  I-1.J-1 
POS(l,1)=POS(l,1)*SF/MAX 
POS(l  ,2)=POS(l  ,2)*S  F/MAX 
POS(l  ,3)=POS(l  ,3)*S  F/M  AX 
CONTINUE 
PRINT  AND  I/O 

REWIND  02 

WRITE  (9,*)  ’CARTESIAN  SCALING  SF=',SF 
WRITE  (2,*)  J 
DO  505  1=1  ,J-1 
WRITE(2,*)(POS(l,L),L=1 ,3) 

WRITE(9,743)  l,(POS(l,L),L=1 ,3) 

CONTINUE 

FORMAT*'  ',T2,I4,2X,3(F5.3,2X)) 

RETURN 


****************************** 

*  SCALE  IN  SPHERICAL  COORDNATES  * 

****************************** 

THE  PURPOSE  OF  THIS  PROGRAM  IS  TO  TAKE  A  SET  OF  SPHERICAL 
COORDINATE  THREE  TUPLE  AND  SCALE  RHO  SO  THAT  THE  LARGEST  VALUE 
IS  ON  THE  SURFACE  OF  A  UNIT  SPHERE  THIS  INSURES  THAT  THE 
MAXIMUM  ATTEMPTED  MOVEMENT  IS  WITHIN  THE  RANGE  OF  THE  MOTION 
OF  THE  ROBOT  ARM  WHEN  THIS  "NORMALIZED"  VALUE  IS  USED  WITH 
A  SCALING  FACTOR  (SF).  WITH  SF  LESS  THAN  OR  EQUAL  TO  ONE  THEN 
THE  MOTION  WILL  BE  WITHIN  THE  UNIT  SPHERE.  A  SF  GREATER  THAN 
ONE  CAN  BE  USED  WITH  THE  ARTICULATED  ROBOT  ARM  WHICH  REQUIRES 
INPUTS  TO  BE  SCALED  TO  THE  ARM  LENGTHS.  WHEN  USED  IN 
CONJUNCTION  WITH  THE  ARTICULATED  PROGRAM  THE  SHPERICAL  COORD 
MUST  BE  WITHIN  THE  UNIT  SPHERE.  A  UNIT  SPHERE  IS  DEFINED  TO 
HAVE  A  DIAMETER  OF  1  UNIT. 

SUBROUTINE  SPHSC(SF.MAX) 

INTEGER  l,J 
REAL  SF, MAX 

DIMENSION  SPPOS{1 000,3) 

REWIND  5 

INPUT  SPHERICAL  PATH 
READ(5,*)  J 
DO306I-1.J-1 

READfS.'MSPPOSO.g.UI  ,3) 

IF  (SPPOS(l,1  J.GT.MAX)  THEN 

MAX-SPPOS(I.I) 

ENDIF 

CONTINUE 

SCALE 

IF  (MAX.EQ.0.0)  THEN 

GOTO  456 

ENDIF 

DO406M.J-1 

SPPOS(l,1  )=SPPOS(l,1  )*SF/(2*MAX) 

CONTINUE 
PRINT  AND  I/O 

REWIND  05 

WRITE  (9,*)  'SPHERICAL  SCALING  SF-'.SF/  MAX  MAX 
WRITE  (5,*)  J 
DO  506  I-1.J-1 

WRITE(5,*)(SPPOS(l,L),L-1 ,3) 

CONTINUE 

FORMATC  \T2,I4,2X,3(F5.3,2X)) 

RETURN 


oooooooooo  o  o  o  ooooooooooo 


END 


*  UNSCALE  IN  SPHERICAL  COORDINATES  ’ 


**•**•*******«•*-•*•***+•******#■•* 
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THE  PURPOSE  OF  THIS  PROGRAM  IS  TO  TAKE  A  SET  OF  SPHERICAL 
COORDINATE  THREE  TUPLE  AND  UNSCALE  RHO  SO  THAT  THE  VALUES  ARE 
CLOSE  TO  WHAT  THEY  WERE  PRIOR  TO  SENDING  THEM  TO  THE  ROBOT 
ARM.  THIS  WAY  THE  FINAL  RESULT  IN  CARTESIAN  WILL  COMPARE  WITH 
THE  VALUES  THAT  THEY  STARTED  WITH.  THIS  IS  DONE  BY  USING 
A  SCAUNG  FACTOR  (SF). 

SUBROUTINE  USPSC(SF) 

INTEGER  l,J 
REALSF 

DIMENSION  SPPOS(1 000,3) 

REWIND  15 

INPUT  SPHERICAL  PATH 
READ(15,*)  J 
DO306I-1.J-1 

READ(15,*)(SPPOS(l,L),L-1 ,3) 

CONTINUE 


SCALE 


406 


506 

743 

456 

END 


DO  406  I-1.J-1 
SPPOS(l,1)-SPPOS(l,1)*SF 
CONTINUE 
PRINT  AND  I/O 

REWIND  15 

WRITE  (9,*)  'SPHERICAL  UN-SCALING  SF-'.SF 
WRITE  (15,*)  J 
DO  506  I-1.J-1 

WRITE(15,*)(SPPOS(l,L),L-1 ,3) 

CONTINUE 

FORMAT('  \T2II4,2XI3(F5.3,2X)) 

RETURN 


•*«*********«  •*++****••«•****« 


LIMIT  IN  CARTESIAN  COORDNATES  ’ 


•**••***«•**«*********#**«#**# 


THE  PURPOSE  OF  THIS  PROGRAM  IS  TO  TAKE  A  SET  OF  CARTESIAN 
COORDINATE  THREE  TUPLE  AND  LIMIT  VALUES  LARGER  THAN  ONE  SO  THEY 
ARE  ON  THE  SURFACE  OF  A  (1 ,1 ,1 )  CUBE.  THIS  INSURES  THAT  THE 
MAXIMUM  ATTEMPTED  MOVEMENT  IS  WITHIN  THE  RANGE  OF  THE  MOTION 
OF  THE  ROBOT  ARM. 
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SUBROUTINE  CARL! 

INTEGER  l,J 

DIMENSION  POS(1 000,3) 

REWIND  2 

C  INPUT  CARTESIAN  PATH 
READ(2,*)  J 
DO  307  1-1  ,J-1 
READ(2,*)(POS(l,L),U1,3) 

IF  (POS(I.I).GT.I.O)  THEN 

POS(I,1)-1.0 

ENDIF 

IF  (POS(I,2).GT.1.0)  THEN 

POS(I,2)-1.0 

ENDIF 

IF  (POS(I,3).GT.1.0)  THEN 

POS(I,3)-1.0 

ENDIF 

307  CONTINUE 

C  PRINT  AND  I/O 

REWIND  02 

WRITE  (9,*)  ’CARTESIAN  LIMITING’ 

WRITE  (2,*)  J 
DO  507  I-1.J-1 
WR!TE(2,*)(POS(l,L),L»1 ,3) 

507  CONTINUE 

743  FORMATC  ’,T2,I4I2X,3(F5.3,2X)) 

RETURN 

END 

#***•••*««•******•*••***•«*** 

*  UMIT  IN  SPHERICAL  COORDNATES  * 

***«•**•*+****♦***••*«*•**•«* 

THE  PURPOSE  OF  THIS  PROGRAM  IS  TO  TAKE  A  SET  OF  SPHERICAL 
COORDINATE  THREE  TUPLE  AND  UMIT  RHO  SO  THAT  THE  LARGEST  VALUE 
IS  ON  THE  SURFACE  OF  A  UNIT  SPHERE  THIS  INSURES  THAT  THE 
MAXIMUM  ATTEMPTED  MOVEMENT  IS  WITHIN  THE  RANGE  OF  THE  MOTION 
OF  THE  ROBOT  ARM. 

SUBROUTINE  SPHU 
INTEGER  I.J 

DIMENSION  SPPOS<1 000,3) 

REWIND  5 

C  INPUT  SPHERICAL  PATH 
READ(5,*)  J 
DO  308  1-1  ,J-1 

READ(5,*)(SPPOS(l,L),L-1 ,3) 


IF  (SPPOS(l ,  1 ) .GT.0.5)  THEN 

SPPOS<I.1)«0.5 

ENDIF 

308  CONTINUE 

C  PRINT  AND  I/O 

REWIND  05 

WRITE  (9,*)  'SPHERICAL  LIMITING* 
WRITE  (5,*)  J 
DO  508  I-1.J-1 

WRITE<5,*)(SPPOS(l,L),L-1 ,3) 

508  CONTfJUE 

743  FORMAT(*  ,,T2II4,2X,3(F5.3,2X)) 

RETURN 
END 


the  text  but  may  be  of  interest  to  the  reader. 
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(c)  Circular  Path  (Reduced) 


PATH  ERROR 


Plane  Projection  and  X,Y,  and  Z  Path  Errors. 


_ path  error 

(Reduced),  STEP  Excitation,  Velocity  Method. 
Plane  Projection  and  X,Y,  and  Z  Path  Error* 


*  SEI 


TIME  (10»SEC1 

_ _ PftTH  ERROR 

yure  rl4{u)  Helix  Path  I  (Reduced),  STEP  Excitation,  Velocity  Method, 
Volmin  =  IO  O  mil  1  i -in,  XY  Plane  Projection  ond  X,Y,  and  1  Poth  Errors. 
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